diff --git a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp index e2ffff2195645..ba18b7dadd599 100644 --- a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp @@ -155,7 +155,6 @@ class EKFLocalizer : public rclcpp::Node const HyperParameters params_; - double ekf_rate_; double ekf_dt_; /* process noise variance for discrete model */ diff --git a/localization/ekf_localizer/include/ekf_localizer/ekf_module.hpp b/localization/ekf_localizer/include/ekf_localizer/ekf_module.hpp index 873060c75fcfc..e88a59ffdfab9 100644 --- a/localization/ekf_localizer/include/ekf_localizer/ekf_module.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/ekf_module.hpp @@ -30,6 +30,7 @@ #include #include +#include struct EKFDiagnosticInfo { @@ -76,12 +77,15 @@ class EKFModule std::array getCurrentPoseCovariance() const; std::array getCurrentTwistCovariance() const; + size_t find_closest_delay_time_index(double target_value) const; + void accumulate_delay_time(const double dt); + void predictWithDelay(const double dt); bool measurementUpdatePose( - const PoseWithCovariance & pose, const double dt, const rclcpp::Time & t_curr, + const PoseWithCovariance & pose, const rclcpp::Time & t_curr, EKFDiagnosticInfo & pose_diag_info); bool measurementUpdateTwist( - const TwistWithCovariance & twist, const double dt, const rclcpp::Time & t_curr, + const TwistWithCovariance & twist, const rclcpp::Time & t_curr, EKFDiagnosticInfo & twist_diag_info); geometry_msgs::msg::PoseWithCovarianceStamped compensatePoseWithZDelay( const PoseWithCovariance & pose, const double delay_time); @@ -91,6 +95,7 @@ class EKFModule std::shared_ptr warning_; const int dim_x_; + std::vector accumulated_delay_times_; const HyperParameters params_; }; diff --git a/localization/ekf_localizer/include/ekf_localizer/warning_message.hpp b/localization/ekf_localizer/include/ekf_localizer/warning_message.hpp index f6c664eb26451..e1eafdc6f7948 100644 --- a/localization/ekf_localizer/include/ekf_localizer/warning_message.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/warning_message.hpp @@ -17,10 +17,9 @@ #include -std::string poseDelayStepWarningMessage( - const double delay_time, const int extend_state_step, const double ekf_dt); +std::string poseDelayStepWarningMessage(const double delay_time, const double delay_time_threshold); std::string twistDelayStepWarningMessage( - const double delay_time, const int extend_state_step, const double ekf_dt); + const double delay_time, const double delay_time_threshold); std::string poseDelayTimeWarningMessage(const double delay_time); std::string twistDelayTimeWarningMessage(const double delay_time); std::string mahalanobisWarningMessage(const double distance, const double max_distance); diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp index 5dd78a00f2fbd..a3d2f52a4058c 100644 --- a/localization/ekf_localizer/src/ekf_localizer.cpp +++ b/localization/ekf_localizer/src/ekf_localizer.cpp @@ -44,7 +44,6 @@ EKFLocalizer::EKFLocalizer(const std::string & node_name, const rclcpp::NodeOpti : rclcpp::Node(node_name, node_options), warning_(std::make_shared(this)), params_(this), - ekf_rate_(params_.ekf_rate), ekf_dt_(params_.ekf_dt), pose_queue_(params_.pose_smoothing_steps), twist_queue_(params_.twist_smoothing_steps) @@ -109,9 +108,22 @@ void EKFLocalizer::updatePredictFrequency() if (get_clock()->now() < *last_predict_time_) { warning_->warn("Detected jump back in time"); } else { - 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); + /* Measure dt */ + ekf_dt_ = (get_clock()->now() - *last_predict_time_).seconds(); + DEBUG_INFO( + get_logger(), "[EKF] update ekf_dt_ to %f seconds (= %f hz)", ekf_dt_, 1 / ekf_dt_); + + if (ekf_dt_ > 10.0) { + ekf_dt_ = 10.0; + RCLCPP_WARN( + get_logger(), "Large ekf_dt_ detected!! (%f sec) Capped to 10.0 seconds", ekf_dt_); + } else if (ekf_dt_ > params_.pose_smoothing_steps / params_.ekf_rate) { + RCLCPP_WARN( + get_logger(), "EKF period may be too slow to finish pose smoothing!! (%f sec) ", ekf_dt_); + } + + /* Register dt and accumulate time delay */ + ekf_module_->accumulate_delay_time(ekf_dt_); /* Update discrete proc_cov*/ proc_cov_vx_d_ = std::pow(params_.proc_stddev_vx_c * ekf_dt_, 2.0); @@ -165,7 +177,7 @@ 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(); - bool is_updated = ekf_module_->measurementUpdatePose(*pose, ekf_dt_, t_curr, pose_diag_info_); + bool is_updated = ekf_module_->measurementUpdatePose(*pose, t_curr, pose_diag_info_); if (is_updated) { pose_is_updated = true; @@ -200,8 +212,7 @@ 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(); - bool is_updated = - ekf_module_->measurementUpdateTwist(*twist, ekf_dt_, t_curr, twist_diag_info_); + bool is_updated = ekf_module_->measurementUpdateTwist(*twist, t_curr, twist_diag_info_); if (is_updated) { twist_is_updated = true; } diff --git a/localization/ekf_localizer/src/ekf_module.cpp b/localization/ekf_localizer/src/ekf_module.cpp index 0a5e3c98c96c8..10926b04507a0 100644 --- a/localization/ekf_localizer/src/ekf_module.cpp +++ b/localization/ekf_localizer/src/ekf_module.cpp @@ -38,6 +38,7 @@ EKFModule::EKFModule(std::shared_ptr warning, const HyperParameters params) : warning_(std::move(warning)), dim_x_(6), // x, y, yaw, yaw_bias, vx, wz + accumulated_delay_times_(params.extend_state_step, 1.0E15), params_(params) { Eigen::MatrixXd X = Eigen::MatrixXd::Zero(dim_x_, 1); @@ -131,6 +132,48 @@ double EKFModule::getYawBias() const return kalman_filter_.getLatestX()(IDX::YAWB); } +size_t EKFModule::find_closest_delay_time_index(double target_value) const +{ + // If target_value is too large, return last index + 1 + if (target_value > accumulated_delay_times_.back()) { + return accumulated_delay_times_.size(); + } + + auto lower = std::lower_bound( + accumulated_delay_times_.begin(), accumulated_delay_times_.end(), target_value); + + // If the lower bound is the first element, return its index. + // If the lower bound is beyond the last element, return the last index. + // If else, take the closest element. + if (lower == accumulated_delay_times_.begin()) { + return 0; + } else if (lower == accumulated_delay_times_.end()) { + return accumulated_delay_times_.size() - 1; + } else { + // Compare the target with the lower bound and the previous element. + auto prev = lower - 1; + bool is_closer_to_prev = (target_value - *prev) < (*lower - target_value); + + // Return the index of the closer element. + return is_closer_to_prev ? std::distance(accumulated_delay_times_.begin(), prev) + : std::distance(accumulated_delay_times_.begin(), lower); + } +} + +void EKFModule::accumulate_delay_time(const double dt) +{ + // Shift the delay times to the right. + std::copy_backward( + accumulated_delay_times_.begin(), accumulated_delay_times_.end() - 1, + accumulated_delay_times_.end()); + + // Add the new delay time to all elements. + accumulated_delay_times_.front() = 0.0; + for (auto & delay_time : accumulated_delay_times_) { + delay_time += dt; + } +} + void EKFModule::predictWithDelay(const double dt) { const Eigen::MatrixXd X_curr = kalman_filter_.getLatestX(); @@ -147,8 +190,7 @@ void EKFModule::predictWithDelay(const double dt) } bool EKFModule::measurementUpdatePose( - const PoseWithCovariance & pose, const double dt, const rclcpp::Time & t_curr, - EKFDiagnosticInfo & pose_diag_info) + const PoseWithCovariance & pose, const rclcpp::Time & t_curr, EKFDiagnosticInfo & pose_diag_info) { if (pose.header.frame_id != params_.pose_frame_id) { warning_->warnThrottle( @@ -170,14 +212,15 @@ bool EKFModule::measurementUpdatePose( delay_time = std::max(delay_time, 0.0); - const int delay_step = std::roundf(delay_time / dt); + const int delay_step = static_cast(find_closest_delay_time_index(delay_time)); pose_diag_info.delay_time = std::max(delay_time, pose_diag_info.delay_time); - pose_diag_info.delay_time_threshold = params_.extend_state_step * dt; + pose_diag_info.delay_time_threshold = accumulated_delay_times_.back(); if (delay_step >= params_.extend_state_step) { pose_diag_info.is_passed_delay_gate = false; warning_->warnThrottle( - poseDelayStepWarningMessage(delay_time, params_.extend_state_step, dt), 2000); + poseDelayStepWarningMessage(pose_diag_info.delay_time, pose_diag_info.delay_time_threshold), + 2000); return false; } @@ -243,7 +286,7 @@ geometry_msgs::msg::PoseWithCovarianceStamped EKFModule::compensatePoseWithZDela } bool EKFModule::measurementUpdateTwist( - const TwistWithCovariance & twist, const double dt, const rclcpp::Time & t_curr, + const TwistWithCovariance & twist, const rclcpp::Time & t_curr, EKFDiagnosticInfo & twist_diag_info) { if (twist.header.frame_id != "base_link") { @@ -262,14 +305,16 @@ bool EKFModule::measurementUpdateTwist( } delay_time = std::max(delay_time, 0.0); - const int delay_step = std::roundf(delay_time / dt); + const int delay_step = static_cast(find_closest_delay_time_index(delay_time)); twist_diag_info.delay_time = std::max(delay_time, twist_diag_info.delay_time); - twist_diag_info.delay_time_threshold = params_.extend_state_step * dt; + twist_diag_info.delay_time_threshold = accumulated_delay_times_.back(); if (delay_step >= params_.extend_state_step) { twist_diag_info.is_passed_delay_gate = false; warning_->warnThrottle( - twistDelayStepWarningMessage(delay_time, params_.extend_state_step, dt), 2000); + twistDelayStepWarningMessage( + twist_diag_info.delay_time, twist_diag_info.delay_time_threshold), + 2000); return false; } diff --git a/localization/ekf_localizer/src/warning_message.cpp b/localization/ekf_localizer/src/warning_message.cpp index 9a7bbf47a94eb..c69f30b2d6601 100644 --- a/localization/ekf_localizer/src/warning_message.cpp +++ b/localization/ekf_localizer/src/warning_message.cpp @@ -18,22 +18,20 @@ #include -std::string poseDelayStepWarningMessage( - const double delay_time, const int extend_state_step, const double ekf_dt) +std::string poseDelayStepWarningMessage(const double delay_time, const double delay_time_threshold) { const std::string s = "Pose delay exceeds the compensation limit, ignored. " - "delay: {:.3f}[s], limit = extend_state_step * ekf_dt : {:.3f}[s]"; - return fmt::format(s, delay_time, extend_state_step * ekf_dt); + "delay: {:.3f}[s], limit: {:.3f}[s]"; + return fmt::format(s, delay_time, delay_time_threshold); } -std::string twistDelayStepWarningMessage( - const double delay_time, const int extend_state_step, const double ekf_dt) +std::string twistDelayStepWarningMessage(const double delay_time, const double delay_time_threshold) { const std::string s = "Twist delay exceeds the compensation limit, ignored. " - "delay: {:.3f}[s], limit = extend_state_step * ekf_dt : {:.3f}[s]"; - return fmt::format(s, delay_time, extend_state_step * ekf_dt); + "delay: {:.3f}[s], limit: {:.3f}[s]"; + return fmt::format(s, delay_time, delay_time_threshold); } std::string poseDelayTimeWarningMessage(const double delay_time) diff --git a/localization/ekf_localizer/test/test_warning_message.cpp b/localization/ekf_localizer/test/test_warning_message.cpp index b4a05afa844dc..2069969e0ae5e 100644 --- a/localization/ekf_localizer/test/test_warning_message.cpp +++ b/localization/ekf_localizer/test/test_warning_message.cpp @@ -21,17 +21,17 @@ TEST(PoseDelayStepWarningMessage, SmokeTest) { EXPECT_STREQ( - poseDelayStepWarningMessage(6.0, 2, 2.0).c_str(), + poseDelayStepWarningMessage(6.0, 4.0).c_str(), "Pose delay exceeds the compensation limit, ignored. " - "delay: 6.000[s], limit = extend_state_step * ekf_dt : 4.000[s]"); + "delay: 6.000[s], limit: 4.000[s]"); } TEST(TwistDelayStepWarningMessage, SmokeTest) { EXPECT_STREQ( - twistDelayStepWarningMessage(10.0, 3, 2.0).c_str(), + twistDelayStepWarningMessage(10.0, 6.0).c_str(), "Twist delay exceeds the compensation limit, ignored. " - "delay: 10.000[s], limit = extend_state_step * ekf_dt : 6.000[s]"); + "delay: 10.000[s], limit: 6.000[s]"); } TEST(PoseDelayTimeWarningMessage, SmokeTest)