Autoware 定位之EKF 滤波定位（四）

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

0. 简介

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

在这里插入图片描述

1. 主要功能

输入消息的时间延迟补偿，可以有效整合具有不同时间延迟的输入信息。这对于高速移动的机器人，比如自动驾驶车辆，尤为重要（见下图）。
自动估计偏航误差，可以防止由于传感器安装角度误差导致的建模错误，从而提高估计精度。
马氏距离门，可以进行概率异常值检测，确定哪些输入应该被使用或忽略。
平滑更新，卡尔曼滤波器的测量更新通常在获得测量值时执行，但这可能导致估计值的大幅变化，尤其是对于低频测量。由于算法可以考虑测量时间，测量数据可以被分成多个部分，并在保持一致性的同时平滑集成（见下图）。
从俯仰角计算垂直校正量，可以减轻坡度上的定位不稳定性。例如，上坡时，由于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 onusing 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 loopconst 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// poseconst 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次获取父坐标系到子坐标系的变换关系，如果成功则返回truereturn 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;
}