diff --git a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp index e972b316626ab..0a7a89a910202 100644 --- a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp @@ -163,8 +163,8 @@ class EKFLocalizer : public rclcpp::Node AgedObjectQueue pose_queue_; AgedObjectQueue twist_queue_; - //!< @brief accumulated time for timer callback - rclcpp::Duration accumulated_time_; + //!< @brief accumulated time for prediction in timer callback + rclcpp::Duration accumulated_lap_time_; /** * @brief computes update & prediction of EKF for each ekf_dt_[s] time @@ -192,9 +192,9 @@ class EKFLocalizer : public rclcpp::Node void callbackInitialPose(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); /** - * @brief update predict frequency + * @brief measure lap time of timer callback */ - void updatePredictFrequency(); + void measureLapTime(); /** * @brief get transform from frame_id diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp index 9aee78566c866..35ba82c5b6a72 100644 --- a/localization/ekf_localizer/src/ekf_localizer.cpp +++ b/localization/ekf_localizer/src/ekf_localizer.cpp @@ -46,7 +46,7 @@ EKFLocalizer::EKFLocalizer(const std::string & node_name, const rclcpp::NodeOpti params_(this), pose_queue_(params_.pose_smoothing_steps), twist_queue_(params_.twist_smoothing_steps), - accumulated_time_(0, 0) + accumulated_lap_time_(0, 0) { is_activated_ = false; @@ -95,18 +95,18 @@ EKFLocalizer::EKFLocalizer(const std::string & node_name, const rclcpp::NodeOpti } /* - * updatePredictFrequency + * measureLapTime */ -void EKFLocalizer::updatePredictFrequency() +void EKFLocalizer::measureLapTime() { rclcpp::Time current_time = get_clock()->now(); if (last_predict_time_.seconds() != 0.0) { if (current_time < last_predict_time_) { warning_->warn("Detected jump back in time"); } else { - accumulated_time_ = accumulated_time_ + (current_time - last_predict_time_); + accumulated_lap_time_ = accumulated_lap_time_ + (current_time - last_predict_time_); double current_ekf_dt_ = (current_time - last_predict_time_).seconds(); - DEBUG_INFO(get_logger(), "[EKF] update ekf_dt_ to %f seconds", current_ekf_dt_); + DEBUG_INFO(get_logger(), "[EKF] timer callback lap time = %f [sec]", current_ekf_dt_); } } last_predict_time_ = current_time; @@ -126,20 +126,20 @@ void EKFLocalizer::timerCallback() DEBUG_INFO(get_logger(), "========================= timer called ========================="); - /* update predict frequency with measured timer rate */ - updatePredictFrequency(); + /* measure lap time of this timer callback and check time jumps */ + measureLapTime(); /* predict model in EKF */ stop_watch_.tic(); DEBUG_INFO(get_logger(), "------------------------- start prediction -------------------------"); - DEBUG_INFO(get_logger(), "[EKF] accumulated_time = %lf [s] (before prediction)", accumulated_time_.seconds()); - size_t prediction_times = static_cast(accumulated_time_.seconds() / params_.ekf_dt); + DEBUG_INFO(get_logger(), "[EKF] accumulated_time = %lf [s] (before prediction)", accumulated_lap_time_.seconds()); + size_t prediction_times = static_cast(accumulated_lap_time_.seconds() / params_.ekf_dt); for (size_t i = 0; i < prediction_times; ++i) { ekf_module_->predictWithDelay(params_.ekf_dt); } - accumulated_time_ = accumulated_time_ - rclcpp::Duration::from_seconds(params_.ekf_dt * prediction_times); + accumulated_lap_time_ = accumulated_lap_time_ - rclcpp::Duration::from_seconds(params_.ekf_dt * prediction_times); DEBUG_INFO(get_logger(), "[EKF] predictKinematicsModel calc time = %f [ms] (predicted %d times)", stop_watch_.toc(), static_cast(prediction_times)); - DEBUG_INFO(get_logger(), "[EKF] accumulated_time = %lf [s] (after prediction)", accumulated_time_.seconds()); + DEBUG_INFO(get_logger(), "[EKF] accumulated_time = %lf [s] (after prediction)", accumulated_lap_time_.seconds()); DEBUG_INFO(get_logger(), "------------------------- end prediction -------------------------\n"); /* pose measurement update */