From 0ad3c9fcb28ec184377712d8184502f0a95a858b Mon Sep 17 00:00:00 2001 From: TaikiYamada4 Date: Tue, 7 Nov 2023 14:40:05 +0900 Subject: [PATCH] Removed ekf_rate_ in ekf_localizer.cpp/hpp Signed-off-by: TaikiYamada4 --- .../ekf_localizer/include/ekf_localizer/ekf_localizer.hpp | 1 - localization/ekf_localizer/src/ekf_localizer.cpp | 6 ++---- 2 files changed, 2 insertions(+), 5 deletions(-) diff --git a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp index eae431eca6d6a..3523bb236e961 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_; bool is_activated_; diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp index dbeef71fefdc5..e7290cee03ccf 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) @@ -104,9 +103,8 @@ 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); + ekf_dt_ = std::min((get_clock()->now() - *last_predict_time_).seconds(), 10.0); + DEBUG_INFO(get_logger(), "[EKF] update ekf_rate_ to %f hz", 1 / ekf_dt_); } } last_predict_time_ = std::make_shared(get_clock()->now());