Removed ekf_rate_ in ekf_localizer.cpp/hpp

Signed-off-by: TaikiYamada4 <[email protected]>
TaikiYamada4 · Nov 7, 2023 · 0ad3c9f · 0ad3c9f
1 parent 15c5f8a
commit 0ad3c9f
Show file tree

Hide file tree

Showing 2 changed files with 2 additions and 5 deletions.
diff --git 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
@@ -44,7 +44,6 @@ EKFLocalizer::EKFLocalizer(const std::string & node_name, const rclcpp::NodeOpti
 : rclcpp::Node(node_name, node_options),
   warning_(std::make_shared<Warning>(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<const rclcpp::Time>(get_clock()->now());