Autoware 定位之EKF 滤波定位(四)

news2024/12/23 23:33:30

Tip: 如果你在进行深度学习、自动驾驶、模型推理、微调或AI绘画出图等任务,并且需要GPU资源,可以考虑使用UCloud云计算旗下的Compshare的GPU算力云平台。他们提供高性价比的4090 GPU,按时收费每卡2.6元,月卡只需要1.7元每小时,并附带200G的免费磁盘空间。通过链接注册并联系客服,可以获得20元代金券(相当于6-7H的免费GPU资源)。欢迎大家体验一下~

0. 简介

对于AutoWare,我们已经讲了很多了,从这一篇我们开始进入实战,按照《Autoware 技术代码解读(三)》梳理的顺序,我们一个个慢慢来看与学习,首先第一个就是基于ekf的滤波定位。扩展卡尔曼滤波定位器通过将二维车辆动力学模型与输入的自我姿态和自我扭矩信息进行整合,估计出鲁棒且噪音较小的机器人姿态和扭矩。

在这里插入图片描述

1. 主要功能

  1. 输入消息的时间延迟补偿,可以有效整合具有不同时间延迟的输入信息。这对于高速移动的机器人,比如自动驾驶车辆,尤为重要(见下图)。
  2. 自动估计偏航误差,可以防止由于传感器安装角度误差导致的建模错误,从而提高估计精度。
  3. 马氏距离门,可以进行概率异常值检测,确定哪些输入应该被使用或忽略。
  4. 平滑更新,卡尔曼滤波器的测量更新通常在获得测量值时执行,但这可能导致估计值的大幅变化,尤其是对于低频测量。由于算法可以考虑测量时间,测量数据可以被分成多个部分,并在保持一致性的同时平滑集成(见下图)。
  5. 从俯仰角计算垂直校正量,可以减轻坡度上的定位不稳定性。例如,上坡时,由于EKF只考虑3DoF(x,y,偏航),它的行为就像被埋在地下一样(见“从俯仰角计算增量”图的左侧)。因此,EKF根据公式校正z坐标(见“从俯仰角计算增量”图的右侧)。

2. 代码阅读

2.1 covariance.cpp

/// @brief 将 EKF 的状态协方差矩阵转换为姿态消息的协方差数组。通过从输入的状态协方差矩阵 P 中提取特定索引位置的值,并将这些值存储到输出的 covariance 数组中,以构建姿态消息的协方差矩阵
/// @param P EKF 的状态协方差矩阵。
/// @return 
std::array<double, 36> ekfCovarianceToPoseMessageCovariance(const Matrix6d & P)
{
  std::array<double, 36> covariance;
  covariance.fill(0.);

  covariance[COV_IDX::X_X] = P(IDX::X, IDX::X);
  covariance[COV_IDX::X_Y] = P(IDX::X, IDX::Y);
  covariance[COV_IDX::X_YAW] = P(IDX::X, IDX::YAW);
  covariance[COV_IDX::Y_X] = P(IDX::Y, IDX::X);
  covariance[COV_IDX::Y_Y] = P(IDX::Y, IDX::Y);
  covariance[COV_IDX::Y_YAW] = P(IDX::Y, IDX::YAW);
  covariance[COV_IDX::YAW_X] = P(IDX::YAW, IDX::X);
  covariance[COV_IDX::YAW_Y] = P(IDX::YAW, IDX::Y);
  covariance[COV_IDX::YAW_YAW] = P(IDX::YAW, IDX::YAW);

  return covariance;
}
/// @brief 将 EKF 的状态协方差矩阵转换为扭转消息的协方差数组
/// @param P EKF 的状态协方差矩阵。
/// @return 
std::array<double, 36> ekfCovarianceToTwistMessageCovariance(const Matrix6d & P)
{
  std::array<double, 36> covariance;
  covariance.fill(0.);

  covariance[COV_IDX::X_X] = P(IDX::VX, IDX::VX);
  covariance[COV_IDX::X_YAW] = P(IDX::VX, IDX::WZ);
  covariance[COV_IDX::YAW_X] = P(IDX::WZ, IDX::VX);
  covariance[COV_IDX::YAW_YAW] = P(IDX::WZ, IDX::WZ);

  return covariance;
}

2.2 diagnostics.cpp

/// @brief 检查进程是否已激活,并返回相应的诊断状态.根据传入的is_activated参数设置stat.values、stat.level和stat.message
/// @param is_activated 进程是否已激活
/// @return 
diagnostic_msgs::msg::DiagnosticStatus checkProcessActivated(const bool is_activated)
{
  diagnostic_msgs::msg::DiagnosticStatus stat;

  diagnostic_msgs::msg::KeyValue key_value;
  key_value.key = "is_activated";
  key_value.value = is_activated ? "True" : "False";
  stat.values.push_back(key_value);

  stat.level = diagnostic_msgs::msg::DiagnosticStatus::OK;
  stat.message = "OK";
  if (!is_activated) {
    stat.level = diagnostic_msgs::msg::DiagnosticStatus::WARN;
    stat.message = "[WARN]process is not activated";
  }

  return stat;
}
/// @brief 检查测量数据是否更新,并返回相应的诊断状态。根据传入的no_update_count参数设置stat.values、stat.level和stat.message
/// @param measurement_type 测量类型
/// @param no_update_count 没有更新的计数
/// @param no_update_count_threshold_warn 警告阈值
/// @param no_update_count_threshold_error 错误阈值
/// @return 
diagnostic_msgs::msg::DiagnosticStatus checkMeasurementUpdated(
  const std::string & measurement_type, const size_t no_update_count,
  const size_t no_update_count_threshold_warn, const size_t no_update_count_threshold_error)
{
  diagnostic_msgs::msg::DiagnosticStatus stat;
    //将各种数据转换为字符串形式,存入 stat.values 中。
  diagnostic_msgs::msg::KeyValue key_value;
  key_value.key = measurement_type + "_no_update_count";
  key_value.value = std::to_string(no_update_count);
  stat.values.push_back(key_value);
  key_value.key = measurement_type + "_no_update_count_threshold_warn";
  key_value.value = std::to_string(no_update_count_threshold_warn);
  stat.values.push_back(key_value);
  key_value.key = measurement_type + "_no_update_count_threshold_error";
  key_value.value = std::to_string(no_update_count_threshold_error);
  stat.values.push_back(key_value);

  stat.level = diagnostic_msgs::msg::DiagnosticStatus::OK;
  stat.message = "OK";
  //根据 no_update_count 是否超过阈值设置 stat.level 和 stat.message。
  if (no_update_count >= no_update_count_threshold_warn) {
    stat.level = diagnostic_msgs::msg::DiagnosticStatus::WARN;
    stat.message = "[WARN]" + measurement_type + " is not updated";
  }
  if (no_update_count >= no_update_count_threshold_error) {
    stat.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR;
    stat.message = "[ERROR]" + measurement_type + " is not updated";
  }

  return stat;
}
/// @brief 检查测量队列的大小,并返回相应的诊断状态
/// @param measurement_type 测量类型
/// @param queue_size 队列大小
/// @return 
diagnostic_msgs::msg::DiagnosticStatus checkMeasurementQueueSize(
  const std::string & measurement_type, const size_t queue_size)
{
  diagnostic_msgs::msg::DiagnosticStatus stat;

  diagnostic_msgs::msg::KeyValue key_value;
  key_value.key = measurement_type + "_queue_size";
  key_value.value = std::to_string(queue_size);
  stat.values.push_back(key_value);

  stat.level = diagnostic_msgs::msg::DiagnosticStatus::OK;
  stat.message = "OK";

  return stat;
}
/// @brief 检查测量是否通过延迟门,并返回相应的诊断状态
/// @param measurement_type 测量类型
/// @param is_passed_delay_gate 表示延迟门是否通过
/// @param delay_time 延迟时间
/// @param delay_time_threshold 延迟时间阈值 
/// @return 
diagnostic_msgs::msg::DiagnosticStatus checkMeasurementDelayGate(
  const std::string & measurement_type, const bool is_passed_delay_gate, const double delay_time,
  const double delay_time_threshold)
{
  diagnostic_msgs::msg::DiagnosticStatus stat;

  diagnostic_msgs::msg::KeyValue key_value;
  key_value.key = measurement_type + "_is_passed_delay_gate";
  key_value.value = is_passed_delay_gate ? "True" : "False";
  stat.values.push_back(key_value);
  key_value.key = measurement_type + "_delay_time";
  key_value.value = std::to_string(delay_time);
  stat.values.push_back(key_value);
  key_value.key = measurement_type + "_delay_time_threshold";
  key_value.value = std::to_string(delay_time_threshold);
  stat.values.push_back(key_value);

  stat.level = diagnostic_msgs::msg::DiagnosticStatus::OK;
  stat.message = "OK";
  if (!is_passed_delay_gate) {
    stat.level = diagnostic_msgs::msg::DiagnosticStatus::WARN;
    stat.message = "[WARN]" + measurement_type + " topic is delay";
  }

  return stat;
}
/// @brief 检查测量是否通过马氏距离门,并返回相应的诊断状态
/// @param measurement_type 测量类型
/// @param is_passed_mahalanobis_gate 表示马氏距离门是否通过 
/// @param mahalanobis_distance 马氏距离
/// @param mahalanobis_distance_threshold  马氏距离阈值 
/// @return 
diagnostic_msgs::msg::DiagnosticStatus checkMeasurementMahalanobisGate(
  const std::string & measurement_type, const bool is_passed_mahalanobis_gate,
  const double mahalanobis_distance, const double mahalanobis_distance_threshold)
{
  diagnostic_msgs::msg::DiagnosticStatus stat;

  diagnostic_msgs::msg::KeyValue key_value;
  key_value.key = measurement_type + "_is_passed_mahalanobis_gate";
  key_value.value = is_passed_mahalanobis_gate ? "True" : "False";
  stat.values.push_back(key_value);
  key_value.key = measurement_type + "_mahalanobis_distance";
  key_value.value = std::to_string(mahalanobis_distance);
  stat.values.push_back(key_value);
  key_value.key = measurement_type + "_mahalanobis_distance_threshold";
  key_value.value = std::to_string(mahalanobis_distance_threshold);
  stat.values.push_back(key_value);

  stat.level = diagnostic_msgs::msg::DiagnosticStatus::OK;
  stat.message = "OK";
  if (!is_passed_mahalanobis_gate) {
    stat.level = diagnostic_msgs::msg::DiagnosticStatus::WARN;
    stat.message = "[WARN]mahalanobis distance of " + measurement_type + " topic is large";
  }

  return stat;
}

// The highest level within the stat_array will be reflected in the merged_stat.
// When all stat_array entries are 'OK,' the message of merged_stat will be "OK"

/// @brief 合并诊断状态数组,并返回合并后的诊断状态
/// @param stat_array 根据每个诊断状态的级别和消息进行合并
/// @return 
diagnostic_msgs::msg::DiagnosticStatus mergeDiagnosticStatus(
  const std::vector<diagnostic_msgs::msg::DiagnosticStatus> & stat_array)
{
  diagnostic_msgs::msg::DiagnosticStatus merged_stat;

  for (const auto & stat : stat_array) {
    if ((stat.level > diagnostic_msgs::msg::DiagnosticStatus::OK)) {
      if (!merged_stat.message.empty()) {
        merged_stat.message += "; ";
      }
      merged_stat.message += stat.message;
    }
    if (stat.level > merged_stat.level) {
      merged_stat.level = stat.level;
    }
    for (const auto & value : stat.values) {
      merged_stat.values.push_back(value);
    }
  }

  if (merged_stat.level == diagnostic_msgs::msg::DiagnosticStatus::OK) {
    merged_stat.message = "OK";
  }

  return merged_stat;
}

2.3 ekf_localizer.cpp

// clang-format off
#define PRINT_MAT(X) std::cout << #X << ":\n" << X << std::endl << std::endl //打印矩阵
#define DEBUG_INFO(...) {if (params_.show_debug_info) {RCLCPP_INFO(__VA_ARGS__);}} //打印调试信息
// clang-format on

using std::placeholders::_1;  // 占位符

/// @brief 构造函数
/// @param node_name 节点名称
/// @param node_options 节点选项
EKFLocalizer::EKFLocalizer(const std::string& node_name, const rclcpp::NodeOptions& node_options)
    : rclcpp::Node(node_name, node_options),
      warning_(std::make_shared<Warning>(
          this)),  // 警告管理器(warning_)、参数配置对象(params_)、EKF的更新频率(ekf_rate_)、时间步长(ekf_dt_)以及姿态和速度队列
      params_(this),
      ekf_rate_(params_.ekf_rate),
      ekf_dt_(params_.ekf_dt),
      pose_queue_(params_.pose_smoothing_steps),
      twist_queue_(params_.twist_smoothing_steps) {
    /* convert to continuous to discrete */
    proc_cov_vx_d_ = std::pow(params_.proc_stddev_vx_c * ekf_dt_, 2.0);
    proc_cov_wz_d_ = std::pow(params_.proc_stddev_wz_c * ekf_dt_, 2.0);
    proc_cov_yaw_d_ = std::pow(params_.proc_stddev_yaw_c * ekf_dt_, 2.0);

    is_activated_ = false;

    // ROS系统的初始化
    /* initialize ros system */
    timer_control_ = rclcpp::create_timer(this,
                                          get_clock(),
                                          rclcpp::Duration::from_seconds(ekf_dt_),
                                          std::bind(&EKFLocalizer::timerCallback, this));

    timer_tf_ = rclcpp::create_timer(this,
                                     get_clock(),
                                     rclcpp::Rate(params_.tf_rate_).period(),
                                     std::bind(&EKFLocalizer::timerTFCallback, this));

    pub_pose_ = create_publisher<geometry_msgs::msg::PoseStamped>("ekf_pose", 1);
    pub_pose_cov_ =
        create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>("ekf_pose_with_covariance",
                                                                        1);
    pub_odom_ = create_publisher<nav_msgs::msg::Odometry>("ekf_odom", 1);
    pub_twist_ = create_publisher<geometry_msgs::msg::TwistStamped>("ekf_twist", 1);
    pub_twist_cov_ = create_publisher<geometry_msgs::msg::TwistWithCovarianceStamped>(
        "ekf_twist_with_covariance",
        1);
    pub_yaw_bias_ =
        create_publisher<tier4_debug_msgs::msg::Float64Stamped>("estimated_yaw_bias", 1);
    pub_biased_pose_ = create_publisher<geometry_msgs::msg::PoseStamped>("ekf_biased_pose", 1);
    pub_biased_pose_cov_ = create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>(
        "ekf_biased_pose_with_covariance",
        1);
    pub_diag_ = this->create_publisher<diagnostic_msgs::msg::DiagnosticArray>("/diagnostics", 10);
    sub_initialpose_ = create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
        "initialpose",
        1,
        std::bind(&EKFLocalizer::callbackInitialPose, this, _1));
    sub_pose_with_cov_ = create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
        "in_pose_with_covariance",
        1,
        std::bind(&EKFLocalizer::callbackPoseWithCovariance, this, _1));
    sub_twist_with_cov_ = create_subscription<geometry_msgs::msg::TwistWithCovarianceStamped>(
        "in_twist_with_covariance",
        1,
        std::bind(&EKFLocalizer::callbackTwistWithCovariance, this, _1));
    service_trigger_node_ =
        create_service<std_srvs::srv::SetBool>("trigger_node_srv",
                                               std::bind(&EKFLocalizer::serviceTriggerNode,
                                                         this,
                                                         std::placeholders::_1,
                                                         std::placeholders::_2),
                                               rclcpp::ServicesQoS().get_rmw_qos_profile());

    tf_br_ = std::make_shared<tf2_ros::TransformBroadcaster>(
        std::shared_ptr<rclcpp::Node>(this, [](auto) {}));

    ekf_module_ = std::make_unique<EKFModule>(warning_, params_);
    logger_configure_ = std::make_unique<tier4_autoware_utils::LoggerLevelConfigure>(this);

    z_filter_.set_proc_dev(1.0);
    roll_filter_.set_proc_dev(0.01);
    pitch_filter_.set_proc_dev(0.01);
}

/// @brief 更新预测频率,根据上次预测时间来更新EKF的预测频率
void EKFLocalizer::updatePredictFrequency() {
    if (last_predict_time_) {
        // 如果检测到时间向后跳跃,则发出警告
        if (get_clock()->now() < *last_predict_time_) {
            warning_->warn("Detected jump back in time");
        } else {
            // 根据时间间隔计算新的EKF频率和时间步长,并更新离散过程噪声方差。
            ekf_rate_ = 1.0 / (get_clock()->now() - *last_predict_time_).seconds();
            DEBUG_INFO(get_logger(), "[EKF] update ekf_rate_ to %f hz", ekf_rate_);
            ekf_dt_ = 1.0 / std::max(ekf_rate_, 0.1);

            /* Update discrete proc_cov*/
            proc_cov_vx_d_ = std::pow(params_.proc_stddev_vx_c * ekf_dt_, 2.0);
            proc_cov_wz_d_ = std::pow(params_.proc_stddev_wz_c * ekf_dt_, 2.0);
            proc_cov_yaw_d_ = std::pow(params_.proc_stddev_yaw_c * ekf_dt_, 2.0);
        }
    }
    last_predict_time_ = std::make_shared<const rclcpp::Time>(get_clock()->now());
}

/// @brief
/// 执行定时回调操作,用于更新预测频率、执行EKF模型的预测和姿态与速度的测量更新,并发布EKF结果。
void EKFLocalizer::timerCallback() {
    // 检查节点是否已激活,若未激活则发出警告并返回
    if (!is_activated_) {
        warning_->warnThrottle(
            "The node is not activated. Provide initial pose to pose_initializer",
            2000);
        publishDiagnostics();
        return;
    }

    DEBUG_INFO(get_logger(), "========================= timer called =========================");

    /* update predict frequency with measured timer rate */
    // 更新预测频率
    updatePredictFrequency();

    /* predict model in EKF */
    // 执行EKF模型的预测
    stop_watch_.tic();
    DEBUG_INFO(get_logger(),
               "------------------------- start prediction -------------------------");
    ekf_module_->predictWithDelay(ekf_dt_);
    DEBUG_INFO(get_logger(), "[EKF] predictKinematicsModel calc time = %f [ms]", stop_watch_.toc());
    DEBUG_INFO(get_logger(),
               "------------------------- end prediction -------------------------\n");

    /* pose measurement update */
    pose_diag_info_.queue_size = pose_queue_.size();
    pose_diag_info_.is_passed_delay_gate = true;
    pose_diag_info_.delay_time = 0.0;
    pose_diag_info_.delay_time_threshold = 0.0;
    pose_diag_info_.is_passed_mahalanobis_gate = true;
    pose_diag_info_.mahalanobis_distance = 0.0;

    bool pose_is_updated = false;

    if (!pose_queue_.empty()) {
        DEBUG_INFO(get_logger(), "------------------------- start Pose -------------------------");
        stop_watch_.tic();

        // save the initial size because the queue size can change in the loop
        const auto t_curr = this->now();
        const size_t n = pose_queue_.size();
        for (size_t i = 0; i < n; ++i) {
            const auto pose = pose_queue_.pop_increment_age();
            // 对姿态和速度进行测量更新,并返回是否更新成功的标志
            bool is_updated =
                ekf_module_->measurementUpdatePose(*pose, ekf_dt_, t_curr, pose_diag_info_);
            if (is_updated) {
                pose_is_updated = true;

                // Update Simple 1D filter with considering change of z value due to measurement
                // pose
                const double delay_time = (t_curr - pose->header.stamp).seconds() +
                                          params_.pose_additional_delay;  // 计算延迟时间
                // 根据测量姿态的时间戳和当前时间戳计算延迟时间,然后使用延迟时间对姿态进行补偿
                const auto pose_with_z_delay =
                    ekf_module_->compensatePoseWithZDelay(*pose, delay_time);
                updateSimple1DFilters(pose_with_z_delay,
                                      params_.pose_smoothing_steps);  // 更新滤波器
            }
        }
        DEBUG_INFO(get_logger(),
                   "[EKF] measurementUpdatePose calc time = %f [ms]",
                   stop_watch_.toc());
        DEBUG_INFO(get_logger(), "------------------------- end Pose -------------------------\n");
    }
    pose_diag_info_.no_update_count = pose_is_updated ? 0 : (pose_diag_info_.no_update_count + 1);

    /* twist measurement update */
    twist_diag_info_.queue_size = twist_queue_.size();
    twist_diag_info_.is_passed_delay_gate = true;
    twist_diag_info_.delay_time = 0.0;
    twist_diag_info_.delay_time_threshold = 0.0;
    twist_diag_info_.is_passed_mahalanobis_gate = true;
    twist_diag_info_.mahalanobis_distance = 0.0;

    bool twist_is_updated = false;

    if (!twist_queue_.empty()) {
        DEBUG_INFO(get_logger(), "------------------------- start Twist -------------------------");
        stop_watch_.tic();

        // 保存初始大小,因为循环中队列大小可能会改变
        const auto t_curr = this->now();
        const size_t n = twist_queue_.size();
        for (size_t i = 0; i < n; ++i) {
            const auto twist = twist_queue_.pop_increment_age();
            bool is_updated = ekf_module_->measurementUpdateTwist(
                *twist,
                ekf_dt_,
                t_curr,
                twist_diag_info_);  // 对姿态和速度进行测量更新,并返回是否更新成功的标志
            if (is_updated) {
                twist_is_updated = true;
            }
        }
        DEBUG_INFO(get_logger(),
                   "[EKF] measurementUpdateTwist calc time = %f [ms]",
                   stop_watch_.toc());
        DEBUG_INFO(get_logger(), "------------------------- end Twist -------------------------\n");
    }
    twist_diag_info_.no_update_count =
        twist_is_updated ? 0 : (twist_diag_info_.no_update_count + 1);  // 更新测量更新次数

    const double z = z_filter_.get_x();
    const double roll = roll_filter_.get_x();
    const double pitch = pitch_filter_.get_x();
    // 获取当前的姿态和速度信息,并发布EKF结果
    const geometry_msgs::msg::PoseStamped current_ekf_pose =
        ekf_module_->getCurrentPose(this->now(), z, roll, pitch, false);
    const geometry_msgs::msg::PoseStamped current_biased_ekf_pose =
        ekf_module_->getCurrentPose(this->now(), z, roll, pitch, true);
    const geometry_msgs::msg::TwistStamped current_ekf_twist =
        ekf_module_->getCurrentTwist(this->now());

    /* publish ekf result */
    publishEstimateResult(current_ekf_pose, current_biased_ekf_pose, current_ekf_twist);
    publishDiagnostics();
}

/// @brief 执行TF变换的定时回调操作,用于获取当前姿态信息并发送TF变换
void EKFLocalizer::timerTFCallback() {
    // 检查节点是否已激活或者姿态框架ID是否为空,若是则直接返回
    if (!is_activated_) {
        return;
    }

    if (params_.pose_frame_id == "") {
        return;
    }

    const double z = z_filter_.get_x();
    const double roll = roll_filter_.get_x();
    const double pitch = pitch_filter_.get_x();
    // 获取当前姿态信息,并将其转换为TF消息并发送
    geometry_msgs::msg::TransformStamped transform_stamped;
    transform_stamped = tier4_autoware_utils::pose2transform(
        ekf_module_->getCurrentPose(this->now(), z, roll, pitch, false),
        "base_link");
    transform_stamped.header.stamp = this->now();
    tf_br_->sendTransform(transform_stamped);
}

/// @brief 从TF缓存中获取两个坐标系之间的变换关系。
/// @param parent_frame 父坐标系名称
/// @param child_frame  子坐标系名称
/// @param transform   变换关系
/// @return 
bool EKFLocalizer::getTransformFromTF(std::string parent_frame,
                                      std::string child_frame,
                                      geometry_msgs::msg::TransformStamped& transform) {
    tf2::BufferCore tf_buffer;
    tf2_ros::TransformListener tf_listener(tf_buffer);//创建TF缓存对象和监听器
    rclcpp::sleep_for(std::chrono::milliseconds(100));

    parent_frame = eraseLeadingSlash(parent_frame);
    child_frame = eraseLeadingSlash(child_frame);

    for (int i = 0; i < 50; ++i) {
        try {
            transform = tf_buffer.lookupTransform(parent_frame, child_frame, tf2::TimePointZero);//循环尝试50次获取父坐标系到子坐标系的变换关系,如果成功则返回true
            return true;
        } catch (tf2::TransformException& ex) {
            // 否则打印警告信息,等待100ms后再次尝试,共循环50次
            warning_->warn(ex.what());
            rclcpp::sleep_for(std::chrono::milliseconds(100));
        }
    }
    return false;
}

/// @brief 处理初始姿态的回调函数,用于初始化EKF模型
/// @param initialpose 初始姿态
void EKFLocalizer::callbackInitialPose(
    geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr initialpose) {
    geometry_msgs::msg::TransformStamped transform;
    // 调用getTransformFromTF()函数获取父坐标系到初始姿态坐标系的变换关系
    if (!getTransformFromTF(params_.pose_frame_id, initialpose->header.frame_id, transform)) {
        RCLCPP_ERROR(get_logger(),
                     "[EKF] TF transform failed. parent = %s, child = %s",
                     params_.pose_frame_id.c_str(),
                     initialpose->header.frame_id.c_str());
    }
    //获取的变换关系和初始姿态信息初始化EKF模型
    ekf_module_->initialize(*initialpose, transform);
    initSimple1DFilters(*initialpose);
}

/// @brief 当接收到包含姿态与协方差信息的消息时,将其存入姿态队列中
/// @param msg 包含姿态与协方差信息的指针
void EKFLocalizer::callbackPoseWithCovariance(
    geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) {
    if (!is_activated_) {
        return;
    }
    //将收到的消息存入姿态队列中,并等待timer回调函数处理
    pose_queue_.push(msg);
}

/// @brief 当接收到包含速度与协方差信息的消息时,根据速度大小判断是否忽略该消息,并在需要时修改协方差信息。
/// @param msg 包含速度与协方差信息的指针
void EKFLocalizer::callbackTwistWithCovariance(
    geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg) {
    // 忽略速度太小的消息。注意,这个不等式不能包含“等于”。
    if (std::abs(msg->twist.twist.linear.x) < params_.threshold_observable_velocity_mps) {
        // 根据速度大小判断是否忽略消息,并在需要时修改协方差信息
        msg->twist.covariance[0 * 6 + 0] = 10000.0;
    }
    twist_queue_.push(msg);
}

/// @brief 发布定位结果,包括姿态、带偏置的姿态、速度以及相应的协方差信息等
/// @param current_ekf_pose 当前估计的姿态
/// @param current_biased_ekf_pose 当前估计的带偏置的姿态
/// @param current_ekf_twist 当前估计的速度
void EKFLocalizer::publishEstimateResult(
    const geometry_msgs::msg::PoseStamped& current_ekf_pose,
    const geometry_msgs::msg::PoseStamped& current_biased_ekf_pose,
    const geometry_msgs::msg::TwistStamped& current_ekf_twist) {
    rclcpp::Time current_time = this->now();

    /* publish latest pose */
    pub_pose_->publish(current_ekf_pose);
    pub_biased_pose_->publish(current_biased_ekf_pose);

    /* publish latest pose with covariance */
    geometry_msgs::msg::PoseWithCovarianceStamped pose_cov;
    pose_cov.header.stamp = current_time;
    pose_cov.header.frame_id = current_ekf_pose.header.frame_id;
    pose_cov.pose.pose = current_ekf_pose.pose;
    pose_cov.pose.covariance = ekf_module_->getCurrentPoseCovariance();
    pub_pose_cov_->publish(pose_cov);

    geometry_msgs::msg::PoseWithCovarianceStamped biased_pose_cov = pose_cov;
    biased_pose_cov.pose.pose = current_biased_ekf_pose.pose;
    pub_biased_pose_cov_->publish(biased_pose_cov);

    /* publish latest twist */
    pub_twist_->publish(current_ekf_twist);

    /* publish latest twist with covariance */
    geometry_msgs::msg::TwistWithCovarianceStamped twist_cov;
    twist_cov.header.stamp = current_time;
    twist_cov.header.frame_id = current_ekf_twist.header.frame_id;
    twist_cov.twist.twist = current_ekf_twist.twist;
    twist_cov.twist.covariance = ekf_module_->getCurrentTwistCovariance();
    pub_twist_cov_->publish(twist_cov);

    /* publish yaw bias */
    tier4_debug_msgs::msg::Float64Stamped yawb;
    yawb.stamp = current_time;
    yawb.data = ekf_module_->getYawBias();
    pub_yaw_bias_->publish(yawb);

    /* publish latest odometry */
    nav_msgs::msg::Odometry odometry;
    odometry.header.stamp = current_time;
    odometry.header.frame_id = current_ekf_pose.header.frame_id;
    odometry.child_frame_id = "base_link";
    odometry.pose = pose_cov.pose;
    odometry.twist = twist_cov.twist;
    pub_odom_->publish(odometry);
}
/// @brief 发布诊断信息,包括激活状态检查、测量更新情况、队列大小、延迟情况、Mahalanobis门限等
void EKFLocalizer::publishDiagnostics() {
    std::vector<diagnostic_msgs::msg::DiagnosticStatus> diag_status_array;

    diag_status_array.push_back(checkProcessActivated(is_activated_));

    if (is_activated_) {
        diag_status_array.push_back(
            checkMeasurementUpdated("pose",
                                    pose_diag_info_.no_update_count,
                                    params_.pose_no_update_count_threshold_warn,
                                    params_.pose_no_update_count_threshold_error));
        diag_status_array.push_back(checkMeasurementQueueSize("pose", pose_diag_info_.queue_size));
        diag_status_array.push_back(
            checkMeasurementDelayGate("pose",
                                      pose_diag_info_.is_passed_delay_gate,
                                      pose_diag_info_.delay_time,
                                      pose_diag_info_.delay_time_threshold));
        diag_status_array.push_back(
            checkMeasurementMahalanobisGate("pose",
                                            pose_diag_info_.is_passed_mahalanobis_gate,
                                            pose_diag_info_.mahalanobis_distance,
                                            params_.pose_gate_dist));

        diag_status_array.push_back(
            checkMeasurementUpdated("twist",
                                    twist_diag_info_.no_update_count,
                                    params_.twist_no_update_count_threshold_warn,
                                    params_.twist_no_update_count_threshold_error));
        diag_status_array.push_back(
            checkMeasurementQueueSize("twist", twist_diag_info_.queue_size));
        diag_status_array.push_back(
            checkMeasurementDelayGate("twist",
                                      twist_diag_info_.is_passed_delay_gate,
                                      twist_diag_info_.delay_time,
                                      twist_diag_info_.delay_time_threshold));
        diag_status_array.push_back(
            checkMeasurementMahalanobisGate("twist",
                                            twist_diag_info_.is_passed_mahalanobis_gate,
                                            twist_diag_info_.mahalanobis_distance,
                                            params_.twist_gate_dist));
    }

    diagnostic_msgs::msg::DiagnosticStatus diag_merged_status;
    diag_merged_status = mergeDiagnosticStatus(diag_status_array);
    diag_merged_status.name = "localization: " + std::string(this->get_name());
    diag_merged_status.hardware_id = this->get_name();

    diagnostic_msgs::msg::DiagnosticArray diag_msg;
    diag_msg.header.stamp = this->now();
    diag_msg.status.push_back(diag_merged_status);
    pub_diag_->publish(diag_msg);
}
/// @brief 更新一维滤波器
/// @param pose 位置和方向的带协方差的姿态消息
/// @param smoothing_step 平滑步数
void EKFLocalizer::updateSimple1DFilters(const geometry_msgs::msg::PoseWithCovarianceStamped& pose,
                                         const size_t smoothing_step) {
    // 从姿态消息中提取高度z和姿态的滚转、俯仰角
    double z = pose.pose.pose.position.z;

    const auto rpy = tier4_autoware_utils::getRPY(pose.pose.pose.orientation);

    //根据姿态消息的协方差矩阵和平滑步数计算高度z、滚转、俯仰角的偏差
    using COV_IDX = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX;
    double z_dev = pose.pose.covariance[COV_IDX::Z_Z] * static_cast<double>(smoothing_step);
    double roll_dev =
        pose.pose.covariance[COV_IDX::ROLL_ROLL] * static_cast<double>(smoothing_step);
    double pitch_dev =
        pose.pose.covariance[COV_IDX::PITCH_PITCH] * static_cast<double>(smoothing_step);

    z_filter_.update(z, z_dev, pose.header.stamp);
    roll_filter_.update(rpy.x, roll_dev, pose.header.stamp);
    pitch_filter_.update(rpy.y, pitch_dev, pose.header.stamp);
}

/// @brief 初始化一维滤波器
/// @param pose 位置和方向的带协方差的姿态消息
void EKFLocalizer::initSimple1DFilters(const geometry_msgs::msg::PoseWithCovarianceStamped& pose) {
    //从姿态消息中提取高度z和姿态的滚转、俯仰角
    double z = pose.pose.pose.position.z;

    const auto rpy = tier4_autoware_utils::getRPY(pose.pose.pose.orientation);

    // 根据姿态消息的协方差矩阵计算高度z、滚转、俯仰角的偏差
    using COV_IDX = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX;
    double z_dev = pose.pose.covariance[COV_IDX::Z_Z];
    double roll_dev = pose.pose.covariance[COV_IDX::ROLL_ROLL];
    double pitch_dev = pose.pose.covariance[COV_IDX::PITCH_PITCH];

    z_filter_.init(z, z_dev, pose.header.stamp);
    roll_filter_.init(rpy.x, roll_dev, pose.header.stamp);
    pitch_filter_.init(rpy.y, pitch_dev, pose.header.stamp);
}

/// @brief 触发节点服务
/// @param req 请求
/// @param res 响应
void EKFLocalizer::serviceTriggerNode(const std_srvs::srv::SetBool::Request::SharedPtr req,
                                      std_srvs::srv::SetBool::Response::SharedPtr res) {
    if (req->data) {
        pose_queue_.clear();
        twist_queue_.clear();
        is_activated_ = true;
    } else {
        is_activated_ = false;
    }
    res->success = true;
    return;
}

2.4 ekf_module.cpp

…详情请参照古月居

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处:http://www.coloradmin.cn/o/1812217.html

如若内容造成侵权/违法违规/事实不符,请联系多彩编程网进行投诉反馈,一经查实,立即删除!

相关文章

网络抓取的最佳用户代理 2024 | 避免在抓取时被禁止使用 UA

你是经常进行网页抓取的人吗&#xff1f;你对你的隐私非常小心吗&#xff1f;那么你一定多次听说过“用户代理”。你知道什么是用户代理吗&#xff1f;它如何影响我们的在线生活呢&#xff1f; 请开始阅读&#xff0c;你会对这篇博客中的一切感兴趣&#xff01; 什么是用户代…

经典文献阅读之--FlashOcc(快速且内存高效的占用预测模块)

Tip: 如果你在进行深度学习、自动驾驶、模型推理、微调或AI绘画出图等任务&#xff0c;并且需要GPU资源&#xff0c;可以考虑使用UCloud云计算旗下的Compshare的GPU算力云平台。他们提供高性价比的4090 GPU&#xff0c;按时收费每卡2.6元&#xff0c;月卡只需要1.7元每小时&…

调用华为API实现语音合成

目录 1.作者介绍2.华为云语音合成2.1 语音合成介绍2.2 华为语音合成服务2.3 应用场景 3. 实验过程以及结果3.1 获取API密钥3.2 调用语音合成算法API3.3 实验代码3.4 运行结果 1.作者介绍 袁斌&#xff0c;男&#xff0c;西安工程大学电子信息学院&#xff0c;2023级研究生 研究…

哥斯拉v4.01webshell实验

1、工具使用 生成有效载荷 选择载荷和加密器 放到对应web目录下&#xff0c;然后就可以添加管理 2 、JAVA_AES_RAW-JSPX/JSP 流量 第一组&#xff1a;请求34880&#xff0c;响应0 第二组&#xff1a;请求48&#xff0c;响应32 第三组&#xff1a;请求64&#xff0c;响应2576…

Navicat访问宝塔中的MySQL

开放数据库权限&#xff1a; 开放3306端口&#xff1a; 连接数据库&#xff1a; 对应好用户名与名称&#xff0c;要不会报错1024&#xff1a;

【每日一题】错误的集合

错误的集合 ✨审题&#xff1a;在一个1-n的数组中&#xff0c;会有一个元素重复&#xff0c;一个元素丢失&#xff1b;&#x1f449;目标;找到重复的元素和丢失的元素并放入一个数组中返还回去 ✨有没有想到单身狗问题的进阶版那个思路&#xff0c;找2个单身狗&#xff0c;一个…

升级和维护老旧LabVIEW程序

在升级老旧LabVIEW程序至64位环境时&#xff0c;需要解决兼容性、性能和稳定性等问题。本文从软件升级、硬件兼容性、程序优化、故障修复等多个角度详细分析。具体包括64位迁移注意事项、修复页面跳转崩溃、解决关闭程序后残留进程的问题&#xff0c;确保程序在新环境中的平稳运…

C++学习日记 | Lecture 7 函数进阶

资料来源&#xff1a;南科大 余仕琪 C/C Program Design LINK&#xff1a; CPP/week07 at main ShiqiYu/CPP GitHub7.1-default-arguments_哔哩哔哩_bilibili7.2-function-overloading_哔哩哔哩_bilibili7.3-function-templates_哔哩哔哩_bilibili7.4-function-pointers-an…

【课程总结】Day7:深度学习概述

前言 本篇文章&#xff0c;我们将通过示例来逐步学习理解导数、求函数最小值、深度学习的本质、以及使用numpy和pytorch实操深度学习训练过程。 线性回归 线性回归内容回顾 在《【课程总结】Day5(下)&#xff1a;PCA降维、SVD分解、聚类算法和集成学习》中&#xff0c;我们…

Diffusers代码学习-ControlNet(Inpaint)

对于Inpaint&#xff0c;需要一个初始图像、一个蒙版图像和一个描述用什么替换蒙版的提示词。ControlNet模型允许添加另一个控制图片来调节模型。让我们用Inpaint蒙版来调整模型。这样&#xff0c;ControlNet可以使用修复掩模作为控件来引导模型在蒙版区域内生成图像。 # 以下代…

【Unity | Editor强化工具】资产快速访问工具

经常在Project窗口中翻找资产相对麻烦&#xff0c;Unity自带的Favorite功能又和Project窗口强绑定&#xff0c;且只能在双列视图下使用&#xff0c;故制作了一个可以在独立窗口中列举常用资产的小工具&#xff1a; Unity Asset Quick Access 。 CSDN弄了个Github加速计划&…

基于深度图像的无监督目标跟踪

概要 大致的步骤 深度图像获取:通过深度传感器(例如ToF相机、双目相机等)获取场景的深度图像。深度图转scanscan转pointcloud点云聚类卡尔曼滤波预测匈牙利算法匹配目标ID更新深度图转scan 参考这篇博客 scan转pointcloud

小迪安全代码语言回溯

java安全 第一个就是文件上传&#xff0c;可以通过../上传到上一级目录&#xff0c;以及别的目录&#xff0c;避免本目录不可以执行 jw令牌窃取 令牌由三部分组成&#xff0c;以.号分割&#xff0c;在java的程序看到cookie是三个奇怪的字符串以.号分开&#xff0c;可以确定是…

snmp-check一键获取SNMP信息(KALI工具系列二十一)

目录 1、KALI LINUX 简介 2、snmp-check工具简介 3、在KALI中使用onesixtyone 3.1 目标主机IP&#xff08;win&#xff09; 3.2 KALI的IP 4、操作示例 4.1 SNMP 检查 4.2 指定 SNMP 端口 4.3 指定社区字符串 4.4 详细输出 4.5 指定多项 5、总结 1、KALI LINUX 简介…

6.11 作业

以下是一个简单的比喻&#xff0c;将多态概念与生活中的实际情况相联系&#xff1a; 比喻&#xff1a;动物园的讲解员和动物表演 想象一下你去了一家动物园&#xff0c;看到了许多不同种类的动物&#xff0c;如狮子、大象、猴子等。现在&#xff0c;动物园里有一位讲解员&…

EGST:Explicit Geometric Structure Transformer论文解读

目录 一、导言 二、相关工作 1、3D局部描述子 2、点云配准方法 三、EGST模型 1、结构化特征 2、特征提取 3、点云匹配 4、变换估计 5、损失函数 四、实验 1、数据集 2、评估指标 3、细节 4、对比实验 一、导言 该论文提出一种基于增强几何结构特征的点云配准方…

【华为 ICT HCIA eNSP 习题汇总】——题目集21

1、OSPF协议中的hello报文不包括以下哪个字段&#xff1f; A、Priority&#xff08;优先级&#xff09; B、Neighbor&#xff08;邻居表&#xff09; C、Interval&#xff08;时间间隔&#xff09; D、Checksum&#xff08;校验和&#xff09; 考点&#xff1a;路由技术原理 解…

三十而已,逐梦AI新赛道,解锁职业第二春

前言 哎&#xff0c;你说这年头&#xff0c;一晃眼就三十了&#xff0c;是不是觉得找工作就像在走钢丝&#xff0c;小心翼翼还生怕踩空&#xff1f;特别是想换个跑道&#xff0c;跑到AI那边去&#xff0c;心里头那个忐忑啊&#xff0c;感觉比相亲还紧张。总担心老板们会不会嫌…

Linux-笔记 设备树插件

前言&#xff1a; 设备树插件&#xff08;Device Tree Blob Overlay&#xff0c;简称 DTBO&#xff09;是Linux内核和嵌入式系统中用于动态修改或扩展系统运行时的设备树配置的一种机制。它是对传统设备&#xff08;Device Tree Source&#xff0c;简称 DTS&#xff09;的补充&…

shiro反序列化分析

shiro反序列化分析 基础知识简单介绍关键组件SecurityManagerSubjectRealm总结 shiro安全框架在web中使用配置文件配置具体实现ShiroFilter过滤器分析 shiro的漏洞shiro550链子分析序列化加密cookie反序列化解密cookie验证总结 poc编写存在的问题和解决CC6TemplatesImlCC6insta…