diff --git a/localization/ekf_localizer/CMakeLists.txt b/localization/ekf_localizer/CMakeLists.txt
index 6e7e194f7cf72..8e29bfed14a28 100644
--- a/localization/ekf_localizer/CMakeLists.txt
+++ b/localization/ekf_localizer/CMakeLists.txt
@@ -16,6 +16,7 @@ ament_auto_find_build_dependencies()
ament_auto_add_library(ekf_localizer_lib SHARED
src/ekf_localizer.cpp
src/covariance.cpp
+ src/diagnostics.cpp
src/mahalanobis.cpp
src/measurement.cpp
src/state_transition.cpp
diff --git a/localization/ekf_localizer/README.md b/localization/ekf_localizer/README.md
index 748b5ee5becc0..977e0fceafd9e 100644
--- a/localization/ekf_localizer/README.md
+++ b/localization/ekf_localizer/README.md
@@ -88,6 +88,10 @@ The parameters and input topic names can be set in the `ekf_localizer.launch` fi
The estimated twist with covariance.
+- diagnostics (diagnostic_msgs/DiagnosticArray)
+
+ The diagnostic information.
+
### Published TF
- base_link
@@ -148,6 +152,15 @@ The parameters are set in `launch/ekf_localizer.launch` .
note: process noise for positions x & y are calculated automatically from nonlinear dynamics.
+### For diagnostics
+
+| Name | Type | Description | Default value |
+| :------------------------------------ | :----- | :----------------------------------------------------------------------------------------------------------------------------------------- | :------------ |
+| pose_no_update_count_threshold_warn | size_t | The threshold at which a WARN state is triggered due to the Pose Topic update not happening continuously for a certain number of times. | 50 |
+| pose_no_update_count_threshold_error | size_t | The threshold at which an ERROR state is triggered due to the Pose Topic update not happening continuously for a certain number of times. | 250 |
+| twist_no_update_count_threshold_warn | size_t | The threshold at which a WARN state is triggered due to the Twist Topic update not happening continuously for a certain number of times. | 50 |
+| twist_no_update_count_threshold_error | size_t | The threshold at which an ERROR state is triggered due to the Twist Topic update not happening continuously for a certain number of times. | 250 |
+
## How to tune EKF parameters
### 0. Preliminaries
@@ -194,6 +207,23 @@ Note that, although the dimension gets larger since the analytical expansion can
+## Diagnostics
+
+
+
+
+
+### The conditions that result in a WARN state
+
+- The node is not in the activate state.
+- The number of consecutive no measurement update via the Pose/Twist topic exceeds the `pose_no_update_count_threshold_warn`/`twist_no_update_count_threshold_warn`.
+- The timestamp of the Pose/Twist topic is beyond the delay compensation range.
+- The Pose/Twist topic is beyond the range of Mahalanobis distance for covariance estimation.
+
+### The conditions that result in an ERROR state
+
+- The number of consecutive no measurement update via the Pose/Twist topic exceeds the `pose_no_update_count_threshold_error`/`twist_no_update_count_threshold_error`.
+
## Known issues
- In the presence of multiple inputs with yaw estimation, yaw bias `b_k` in the current EKF state would not make any sense, since it is intended to capture the extrinsic parameter's calibration error of a sensor. Thus, future work includes introducing yaw bias for each sensor with yaw estimation.
diff --git a/localization/ekf_localizer/config/ekf_localizer.param.yaml b/localization/ekf_localizer/config/ekf_localizer.param.yaml
index 4d3f5b9643462..8b24b79e71829 100644
--- a/localization/ekf_localizer/config/ekf_localizer.param.yaml
+++ b/localization/ekf_localizer/config/ekf_localizer.param.yaml
@@ -21,3 +21,9 @@
proc_stddev_yaw_c: 0.005
proc_stddev_vx_c: 10.0
proc_stddev_wz_c: 5.0
+
+ # for diagnostics
+ pose_no_update_count_threshold_warn: 50
+ pose_no_update_count_threshold_error: 250
+ twist_no_update_count_threshold_warn: 50
+ twist_no_update_count_threshold_error: 250
diff --git a/localization/ekf_localizer/include/ekf_localizer/diagnostics.hpp b/localization/ekf_localizer/include/ekf_localizer/diagnostics.hpp
new file mode 100644
index 0000000000000..f4dc6436f6a40
--- /dev/null
+++ b/localization/ekf_localizer/include/ekf_localizer/diagnostics.hpp
@@ -0,0 +1,40 @@
+// Copyright 2023 Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef EKF_LOCALIZER__DIAGNOSTICS_HPP_
+#define EKF_LOCALIZER__DIAGNOSTICS_HPP_
+
+#include
+
+#include
+#include
+
+diagnostic_msgs::msg::DiagnosticStatus checkProcessActivated(const bool is_activated);
+
+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 checkMeasurementQueueSize(
+ const std::string & measurement_type, const size_t queue_size);
+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 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 mergeDiagnosticStatus(
+ const std::vector & stat_array);
+
+#endif // EKF_LOCALIZER__DIAGNOSTICS_HPP_
diff --git a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp
index a4ae47b670897..4fc2305cc7adc 100644
--- a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp
+++ b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp
@@ -25,6 +25,7 @@
#include
#include
+#include
#include
#include
#include
@@ -126,6 +127,8 @@ class EKFLocalizer : public rclcpp::Node
rclcpp::Publisher::SharedPtr pub_biased_pose_;
//!< @brief ekf estimated yaw bias publisher
rclcpp::Publisher::SharedPtr pub_biased_pose_cov_;
+ //!< @brief diagnostics publisher
+ rclcpp::Publisher::SharedPtr pub_diag_;
//!< @brief initial pose subscriber
rclcpp::Subscription::SharedPtr sub_initialpose_;
//!< @brief measurement pose with covariance subscriber
@@ -144,6 +147,7 @@ class EKFLocalizer : public rclcpp::Node
rclcpp::TimerBase::SharedPtr timer_tf_;
//!< @brief tf broadcaster
std::shared_ptr tf_br_;
+
//!< @brief extended kalman filter instance.
TimeDelayKalmanFilter ekf_;
Simple1DFilter z_filter_;
@@ -167,6 +171,22 @@ class EKFLocalizer : public rclcpp::Node
bool is_activated_;
+ size_t pose_no_update_count_;
+ size_t pose_queue_size_;
+ bool pose_is_passed_delay_gate_;
+ double pose_delay_time_;
+ double pose_delay_time_threshold_;
+ bool pose_is_passed_mahalanobis_gate_;
+ double pose_mahalanobis_distance_;
+
+ size_t twist_no_update_count_;
+ size_t twist_queue_size_;
+ bool twist_is_passed_delay_gate_;
+ double twist_delay_time_;
+ double twist_delay_time_threshold_;
+ bool twist_is_passed_mahalanobis_gate_;
+ double twist_mahalanobis_distance_;
+
AgedObjectQueue pose_queue_;
AgedObjectQueue twist_queue_;
@@ -221,13 +241,13 @@ class EKFLocalizer : public rclcpp::Node
* @brief compute EKF update with pose measurement
* @param pose measurement value
*/
- void measurementUpdatePose(const geometry_msgs::msg::PoseWithCovarianceStamped & pose);
+ bool measurementUpdatePose(const geometry_msgs::msg::PoseWithCovarianceStamped & pose);
/**
* @brief compute EKF update with pose measurement
* @param twist measurement value
*/
- void measurementUpdateTwist(const geometry_msgs::msg::TwistWithCovarianceStamped & twist);
+ bool measurementUpdateTwist(const geometry_msgs::msg::TwistWithCovarianceStamped & twist);
/**
* @brief get transform from frame_id
@@ -246,6 +266,11 @@ class EKFLocalizer : public rclcpp::Node
*/
void publishEstimateResult();
+ /**
+ * @brief publish diagnostics message
+ */
+ void publishDiagnostics();
+
/**
* @brief for debug
*/
diff --git a/localization/ekf_localizer/include/ekf_localizer/hyper_parameters.hpp b/localization/ekf_localizer/include/ekf_localizer/hyper_parameters.hpp
index c74fa9be79525..9fa877c8fd2f6 100644
--- a/localization/ekf_localizer/include/ekf_localizer/hyper_parameters.hpp
+++ b/localization/ekf_localizer/include/ekf_localizer/hyper_parameters.hpp
@@ -39,7 +39,15 @@ class HyperParameters
twist_smoothing_steps(node->declare_parameter("twist_smoothing_steps", 2)),
proc_stddev_vx_c(node->declare_parameter("proc_stddev_vx_c", 5.0)),
proc_stddev_wz_c(node->declare_parameter("proc_stddev_wz_c", 1.0)),
- proc_stddev_yaw_c(node->declare_parameter("proc_stddev_yaw_c", 0.005))
+ proc_stddev_yaw_c(node->declare_parameter("proc_stddev_yaw_c", 0.005)),
+ pose_no_update_count_threshold_warn(
+ node->declare_parameter("pose_no_update_count_threshold_warn", 50)),
+ pose_no_update_count_threshold_error(
+ node->declare_parameter("pose_no_update_count_threshold_error", 250)),
+ twist_no_update_count_threshold_warn(
+ node->declare_parameter("twist_no_update_count_threshold_warn", 50)),
+ twist_no_update_count_threshold_error(
+ node->declare_parameter("twist_no_update_count_threshold_error", 250))
{
}
@@ -59,6 +67,10 @@ class HyperParameters
const double proc_stddev_vx_c; //!< @brief vx process noise
const double proc_stddev_wz_c; //!< @brief wz process noise
const double proc_stddev_yaw_c; //!< @brief yaw process noise
+ const size_t pose_no_update_count_threshold_warn;
+ const size_t pose_no_update_count_threshold_error;
+ const size_t twist_no_update_count_threshold_warn;
+ const size_t twist_no_update_count_threshold_error;
};
#endif // EKF_LOCALIZER__HYPER_PARAMETERS_HPP_
diff --git a/localization/ekf_localizer/media/ekf_diagnostics.png b/localization/ekf_localizer/media/ekf_diagnostics.png
new file mode 100644
index 0000000000000..2580d6d973290
Binary files /dev/null and b/localization/ekf_localizer/media/ekf_diagnostics.png differ
diff --git a/localization/ekf_localizer/package.xml b/localization/ekf_localizer/package.xml
index 5bc9c5e42712d..005bddd3eb22b 100644
--- a/localization/ekf_localizer/package.xml
+++ b/localization/ekf_localizer/package.xml
@@ -18,6 +18,7 @@
eigen
+ diagnostic_msgs
fmt
geometry_msgs
kalman_filter
diff --git a/localization/ekf_localizer/src/diagnostics.cpp b/localization/ekf_localizer/src/diagnostics.cpp
new file mode 100644
index 0000000000000..9ff36561abaa9
--- /dev/null
+++ b/localization/ekf_localizer/src/diagnostics.cpp
@@ -0,0 +1,169 @@
+// Copyright 2023 Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "ekf_localizer/diagnostics.hpp"
+
+#include
+
+#include
+#include
+
+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;
+}
+
+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;
+
+ 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";
+ 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;
+}
+
+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;
+}
+
+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;
+}
+
+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"
+diagnostic_msgs::msg::DiagnosticStatus mergeDiagnosticStatus(
+ const std::vector & 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;
+}
diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp
index 0e46a26add852..68a31bcdded1a 100644
--- a/localization/ekf_localizer/src/ekf_localizer.cpp
+++ b/localization/ekf_localizer/src/ekf_localizer.cpp
@@ -15,6 +15,7 @@
#include "ekf_localizer/ekf_localizer.hpp"
#include "ekf_localizer/covariance.hpp"
+#include "ekf_localizer/diagnostics.hpp"
#include "ekf_localizer/mahalanobis.hpp"
#include "ekf_localizer/matrix_types.hpp"
#include "ekf_localizer/measurement.hpp"
@@ -87,6 +88,7 @@ EKFLocalizer::EKFLocalizer(const std::string & node_name, const rclcpp::NodeOpti
pub_biased_pose_ = create_publisher("ekf_biased_pose", 1);
pub_biased_pose_cov_ = create_publisher(
"ekf_biased_pose_with_covariance", 1);
+ pub_diag_ = this->create_publisher("/diagnostics", 10);
sub_initialpose_ = create_subscription(
"initialpose", 1, std::bind(&EKFLocalizer::callbackInitialPose, this, _1));
sub_pose_with_cov_ = create_subscription(
@@ -143,6 +145,7 @@ void EKFLocalizer::timerCallback()
if (!is_activated_) {
warning_.warnThrottle(
"The node is not activated. Provide initial pose to pose_initializer", 2000);
+ publishDiagnostics();
return;
}
@@ -176,6 +179,16 @@ void EKFLocalizer::timerCallback()
DEBUG_INFO(get_logger(), "------------------------- end prediction -------------------------\n");
/* pose measurement update */
+
+ pose_queue_size_ = pose_queue_.size();
+ pose_is_passed_delay_gate_ = true;
+ pose_delay_time_ = 0.0;
+ pose_delay_time_threshold_ = 0.0;
+ pose_is_passed_mahalanobis_gate_ = true;
+ pose_mahalanobis_distance_ = 0.0;
+
+ bool pose_is_updated = false;
+
if (!pose_queue_.empty()) {
DEBUG_INFO(get_logger(), "------------------------- start Pose -------------------------");
stop_watch_.tic();
@@ -184,13 +197,27 @@ void EKFLocalizer::timerCallback()
const size_t n = pose_queue_.size();
for (size_t i = 0; i < n; ++i) {
const auto pose = pose_queue_.pop_increment_age();
- measurementUpdatePose(*pose);
+ bool is_updated = measurementUpdatePose(*pose);
+ if (is_updated) {
+ pose_is_updated = true;
+ }
}
DEBUG_INFO(get_logger(), "[EKF] measurementUpdatePose calc time = %f [ms]", stop_watch_.toc());
DEBUG_INFO(get_logger(), "------------------------- end Pose -------------------------\n");
}
+ pose_no_update_count_ = pose_is_updated ? 0 : (pose_no_update_count_ + 1);
/* twist measurement update */
+
+ twist_queue_size_ = twist_queue_.size();
+ twist_is_passed_delay_gate_ = true;
+ twist_delay_time_ = 0.0;
+ twist_delay_time_threshold_ = 0.0;
+ twist_is_passed_mahalanobis_gate_ = true;
+ twist_mahalanobis_distance_ = 0.0;
+
+ bool twist_is_updated = false;
+
if (!twist_queue_.empty()) {
DEBUG_INFO(get_logger(), "------------------------- start Twist -------------------------");
stop_watch_.tic();
@@ -199,11 +226,15 @@ void EKFLocalizer::timerCallback()
const size_t n = twist_queue_.size();
for (size_t i = 0; i < n; ++i) {
const auto twist = twist_queue_.pop_increment_age();
- measurementUpdateTwist(*twist);
+ bool is_updated = measurementUpdateTwist(*twist);
+ 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_no_update_count_ = twist_is_updated ? 0 : (twist_no_update_count_ + 1);
const double x = ekf_.getXelement(IDX::X);
const double y = ekf_.getXelement(IDX::Y);
@@ -235,6 +266,7 @@ void EKFLocalizer::timerCallback()
/* publish ekf result */
publishEstimateResult();
+ publishDiagnostics();
}
void EKFLocalizer::showCurrentX()
@@ -376,7 +408,7 @@ void EKFLocalizer::initEKF()
/*
* measurementUpdatePose
*/
-void EKFLocalizer::measurementUpdatePose(const geometry_msgs::msg::PoseWithCovarianceStamped & pose)
+bool EKFLocalizer::measurementUpdatePose(const geometry_msgs::msg::PoseWithCovarianceStamped & pose)
{
if (pose.header.frame_id != params_.pose_frame_id) {
warning_.warnThrottle(
@@ -400,10 +432,14 @@ void EKFLocalizer::measurementUpdatePose(const geometry_msgs::msg::PoseWithCovar
delay_time = std::max(delay_time, 0.0);
int delay_step = std::roundf(delay_time / ekf_dt_);
+
+ pose_delay_time_ = std::max(delay_time, pose_delay_time_);
+ pose_delay_time_threshold_ = params_.extend_state_step * ekf_dt_;
if (delay_step >= params_.extend_state_step) {
+ pose_is_passed_delay_gate_ = false;
warning_.warnThrottle(
poseDelayStepWarningMessage(delay_time, params_.extend_state_step, ekf_dt_), 2000);
- return;
+ return false;
}
DEBUG_INFO(get_logger(), "delay_time: %f [s]", delay_time);
@@ -420,7 +456,7 @@ void EKFLocalizer::measurementUpdatePose(const geometry_msgs::msg::PoseWithCovar
if (hasNan(y) || hasInf(y)) {
warning_.warn(
"[EKF] pose measurement matrix includes NaN of Inf. ignore update. check pose message.");
- return;
+ return false;
}
/* Gate */
@@ -431,10 +467,12 @@ void EKFLocalizer::measurementUpdatePose(const geometry_msgs::msg::PoseWithCovar
const Eigen::MatrixXd P_y = P_curr.block(0, 0, dim_y, dim_y);
const double distance = mahalanobis(y_ekf, y, P_y);
+ pose_mahalanobis_distance_ = std::max(distance, pose_mahalanobis_distance_);
if (distance > params_.pose_gate_dist) {
+ pose_is_passed_mahalanobis_gate_ = false;
warning_.warnThrottle(mahalanobisWarningMessage(distance, params_.pose_gate_dist), 2000);
warning_.warnThrottle("Ignore the measurement data.", 2000);
- return;
+ return false;
}
DEBUG_PRINT_MAT(y.transpose());
@@ -460,12 +498,14 @@ void EKFLocalizer::measurementUpdatePose(const geometry_msgs::msg::PoseWithCovar
const Eigen::MatrixXd X_result = ekf_.getLatestX();
DEBUG_PRINT_MAT(X_result.transpose());
DEBUG_PRINT_MAT((X_result - X_curr).transpose());
+
+ return true;
}
/*
* measurementUpdateTwist
*/
-void EKFLocalizer::measurementUpdateTwist(
+bool EKFLocalizer::measurementUpdateTwist(
const geometry_msgs::msg::TwistWithCovarianceStamped & twist)
{
if (twist.header.frame_id != "base_link") {
@@ -488,10 +528,14 @@ void EKFLocalizer::measurementUpdateTwist(
delay_time = std::max(delay_time, 0.0);
int delay_step = std::roundf(delay_time / ekf_dt_);
+
+ twist_delay_time_ = std::max(delay_time, twist_delay_time_);
+ twist_delay_time_threshold_ = params_.extend_state_step * ekf_dt_;
if (delay_step >= params_.extend_state_step) {
+ twist_is_passed_delay_gate_ = false;
warning_.warnThrottle(
twistDelayStepWarningMessage(delay_time, params_.extend_state_step, ekf_dt_), 2000);
- return;
+ return false;
}
DEBUG_INFO(get_logger(), "delay_time: %f [s]", delay_time);
@@ -502,7 +546,7 @@ void EKFLocalizer::measurementUpdateTwist(
if (hasNan(y) || hasInf(y)) {
warning_.warn(
"[EKF] twist measurement matrix includes NaN of Inf. ignore update. check twist message.");
- return;
+ return false;
}
const Eigen::Vector2d y_ekf(
@@ -512,10 +556,12 @@ void EKFLocalizer::measurementUpdateTwist(
const Eigen::MatrixXd P_y = P_curr.block(4, 4, dim_y, dim_y);
const double distance = mahalanobis(y_ekf, y, P_y);
+ twist_mahalanobis_distance_ = std::max(distance, twist_mahalanobis_distance_);
if (distance > params_.twist_gate_dist) {
+ twist_is_passed_mahalanobis_gate_ = false;
warning_.warnThrottle(mahalanobisWarningMessage(distance, params_.twist_gate_dist), 2000);
warning_.warnThrottle("Ignore the measurement data.", 2000);
- return;
+ return false;
}
DEBUG_PRINT_MAT(y.transpose());
@@ -532,6 +578,8 @@ void EKFLocalizer::measurementUpdateTwist(
const Eigen::MatrixXd X_result = ekf_.getLatestX();
DEBUG_PRINT_MAT(X_result.transpose());
DEBUG_PRINT_MAT((X_result - X_curr).transpose());
+
+ return true;
}
/*
@@ -607,6 +655,45 @@ void EKFLocalizer::publishEstimateResult()
pub_debug_->publish(msg);
}
+void EKFLocalizer::publishDiagnostics()
+{
+ std::vector diag_status_array;
+
+ diag_status_array.push_back(checkProcessActivated(is_activated_));
+
+ if (is_activated_) {
+ diag_status_array.push_back(checkMeasurementUpdated(
+ "pose", pose_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_queue_size_));
+ diag_status_array.push_back(checkMeasurementDelayGate(
+ "pose", pose_is_passed_delay_gate_, pose_delay_time_, pose_delay_time_threshold_));
+ diag_status_array.push_back(checkMeasurementMahalanobisGate(
+ "pose", pose_is_passed_mahalanobis_gate_, pose_mahalanobis_distance_,
+ params_.pose_gate_dist));
+
+ diag_status_array.push_back(checkMeasurementUpdated(
+ "twist", twist_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_queue_size_));
+ diag_status_array.push_back(checkMeasurementDelayGate(
+ "twist", twist_is_passed_delay_gate_, twist_delay_time_, twist_delay_time_threshold_));
+ diag_status_array.push_back(checkMeasurementMahalanobisGate(
+ "twist", twist_is_passed_mahalanobis_gate_, twist_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);
+}
+
void EKFLocalizer::updateSimple1DFilters(
const geometry_msgs::msg::PoseWithCovarianceStamped & pose, const size_t smoothing_step)
{
diff --git a/localization/ekf_localizer/test/test_diagnostics.cpp b/localization/ekf_localizer/test/test_diagnostics.cpp
new file mode 100644
index 0000000000000..f506dca1cb230
--- /dev/null
+++ b/localization/ekf_localizer/test/test_diagnostics.cpp
@@ -0,0 +1,192 @@
+// Copyright 2023 Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "ekf_localizer/diagnostics.hpp"
+
+#include
+
+TEST(TestEkfDiagnostics, CheckProcessActivated)
+{
+ diagnostic_msgs::msg::DiagnosticStatus stat;
+
+ bool is_activated = true;
+ stat = checkProcessActivated(is_activated);
+ EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK);
+
+ is_activated = false;
+ stat = checkProcessActivated(is_activated);
+ EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN);
+}
+
+TEST(TestEkfDiagnostics, checkMeasurementUpdated)
+{
+ diagnostic_msgs::msg::DiagnosticStatus stat;
+
+ const std::string measurement_type = "pose"; // not effect for stat.level
+ const size_t no_update_count_threshold_warn = 50;
+ const size_t no_update_count_threshold_error = 250;
+
+ size_t no_update_count = 0;
+ stat = checkMeasurementUpdated(
+ measurement_type, no_update_count, no_update_count_threshold_warn,
+ no_update_count_threshold_error);
+ EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK);
+
+ no_update_count = 1;
+ stat = checkMeasurementUpdated(
+ measurement_type, no_update_count, no_update_count_threshold_warn,
+ no_update_count_threshold_error);
+ EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK);
+
+ no_update_count = 49;
+ stat = checkMeasurementUpdated(
+ measurement_type, no_update_count, no_update_count_threshold_warn,
+ no_update_count_threshold_error);
+ EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK);
+
+ no_update_count = 50;
+ stat = checkMeasurementUpdated(
+ measurement_type, no_update_count, no_update_count_threshold_warn,
+ no_update_count_threshold_error);
+ EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN);
+
+ no_update_count = 249;
+ stat = checkMeasurementUpdated(
+ measurement_type, no_update_count, no_update_count_threshold_warn,
+ no_update_count_threshold_error);
+ EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN);
+
+ no_update_count = 250;
+ stat = checkMeasurementUpdated(
+ measurement_type, no_update_count, no_update_count_threshold_warn,
+ no_update_count_threshold_error);
+ EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::ERROR);
+}
+
+TEST(TestEkfDiagnostics, CheckMeasurementQueueSize)
+{
+ diagnostic_msgs::msg::DiagnosticStatus stat;
+
+ const std::string measurement_type = "pose"; // not effect for stat.level
+
+ size_t queue_size = 0; // not effect for stat.level
+ stat = checkMeasurementQueueSize(measurement_type, queue_size);
+ EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK);
+
+ queue_size = 1; // not effect for stat.level
+ stat = checkMeasurementQueueSize(measurement_type, queue_size);
+ EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK);
+}
+
+TEST(TestEkfDiagnostics, CheckMeasurementDelayGate)
+{
+ diagnostic_msgs::msg::DiagnosticStatus stat;
+
+ const std::string measurement_type = "pose"; // not effect for stat.level
+ const double delay_time = 0.1; // not effect for stat.level
+ const double delay_time_threshold = 1.0; // not effect for stat.level
+
+ bool is_passed_delay_gate = true;
+ stat = checkMeasurementDelayGate(
+ measurement_type, is_passed_delay_gate, delay_time, delay_time_threshold);
+ EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK);
+
+ is_passed_delay_gate = false;
+ stat = checkMeasurementDelayGate(
+ measurement_type, is_passed_delay_gate, delay_time, delay_time_threshold);
+ EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN);
+}
+
+TEST(TestEkfDiagnostics, CheckMeasurementMahalanobisGate)
+{
+ diagnostic_msgs::msg::DiagnosticStatus stat;
+
+ const std::string measurement_type = "pose"; // not effect for stat.level
+ const double mahalanobis_distance = 0.1; // not effect for stat.level
+ const double mahalanobis_distance_threshold = 1.0; // not effect for stat.level
+
+ bool is_passed_mahalanobis_gate = true;
+ stat = checkMeasurementMahalanobisGate(
+ measurement_type, is_passed_mahalanobis_gate, mahalanobis_distance,
+ mahalanobis_distance_threshold);
+ EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK);
+
+ is_passed_mahalanobis_gate = false;
+ stat = checkMeasurementMahalanobisGate(
+ measurement_type, is_passed_mahalanobis_gate, mahalanobis_distance,
+ mahalanobis_distance_threshold);
+ EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN);
+}
+
+TEST(TestLocalizationErrorMonitorDiagnostics, MergeDiagnosticStatus)
+{
+ diagnostic_msgs::msg::DiagnosticStatus merged_stat;
+ std::vector stat_array(2);
+
+ stat_array.at(0).level = diagnostic_msgs::msg::DiagnosticStatus::OK;
+ stat_array.at(0).message = "OK";
+ stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::OK;
+ stat_array.at(1).message = "OK";
+ merged_stat = mergeDiagnosticStatus(stat_array);
+ EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK);
+ EXPECT_EQ(merged_stat.message, "OK");
+
+ stat_array.at(0).level = diagnostic_msgs::msg::DiagnosticStatus::WARN;
+ stat_array.at(0).message = "WARN0";
+ stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::OK;
+ stat_array.at(1).message = "OK";
+ merged_stat = mergeDiagnosticStatus(stat_array);
+ EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN);
+ EXPECT_EQ(merged_stat.message, "WARN0");
+
+ stat_array.at(0).level = diagnostic_msgs::msg::DiagnosticStatus::OK;
+ stat_array.at(0).message = "OK";
+ stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::WARN;
+ stat_array.at(1).message = "WARN1";
+ merged_stat = mergeDiagnosticStatus(stat_array);
+ EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN);
+ EXPECT_EQ(merged_stat.message, "WARN1");
+
+ stat_array.at(0).level = diagnostic_msgs::msg::DiagnosticStatus::WARN;
+ stat_array.at(0).message = "WARN0";
+ stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::WARN;
+ stat_array.at(1).message = "WARN1";
+ merged_stat = mergeDiagnosticStatus(stat_array);
+ EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN);
+ EXPECT_EQ(merged_stat.message, "WARN0; WARN1");
+
+ stat_array.at(0).level = diagnostic_msgs::msg::DiagnosticStatus::OK;
+ stat_array.at(0).message = "OK";
+ stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::ERROR;
+ stat_array.at(1).message = "ERROR1";
+ merged_stat = mergeDiagnosticStatus(stat_array);
+ EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::ERROR);
+ EXPECT_EQ(merged_stat.message, "ERROR1");
+
+ stat_array.at(0).level = diagnostic_msgs::msg::DiagnosticStatus::WARN;
+ stat_array.at(0).message = "WARN0";
+ stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::ERROR;
+ stat_array.at(1).message = "ERROR1";
+ merged_stat = mergeDiagnosticStatus(stat_array);
+ EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::ERROR);
+ EXPECT_EQ(merged_stat.message, "WARN0; ERROR1");
+
+ stat_array.at(0).level = diagnostic_msgs::msg::DiagnosticStatus::ERROR;
+ stat_array.at(0).message = "ERROR0";
+ stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::ERROR;
+ stat_array.at(1).message = "ERROR1";
+ merged_stat = mergeDiagnosticStatus(stat_array);
+ EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::ERROR);
+ EXPECT_EQ(merged_stat.message, "ERROR0; ERROR1");
+}