From 605896fd3f36298f89587cf60ca9bbb2e2b12c91 Mon Sep 17 00:00:00 2001 From: kosuke55 Date: Wed, 16 Feb 2022 18:58:28 +0900 Subject: [PATCH 1/4] fix(ekf_localizer): update predict frequency with measured timer rate Signed-off-by: kosuke55 --- .../include/ekf_localizer/ekf_localizer.hpp | 23 +++++ .../ekf_localizer/src/ekf_localizer.cpp | 87 ++++++++++++++----- 2 files changed, 89 insertions(+), 21 deletions(-) diff --git a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp index e500e5b91a49a..743a696040a75 100644 --- a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp @@ -37,6 +37,7 @@ #include #include +#include #include #include #include @@ -76,8 +77,12 @@ class EKFLocalizer : public rclcpp::Node //!< @brief measurement twist with covariance subscriber rclcpp::Subscription::SharedPtr sub_twist_with_cov_; + //!< @brief clock + rclcpp::Clock::SharedPtr clock_; //!< @brief time for ekf calculation callback rclcpp::TimerBase::SharedPtr timer_control_; + //!< @brief time buffer for updating ekf rate + std::deque time_buffer_; //!< @brief timer to send transform rclcpp::TimerBase::SharedPtr timer_tf_; //!< @brief tf broadcaster @@ -113,6 +118,12 @@ class EKFLocalizer : public rclcpp::Node //!< @brief measurement is ignored if the mahalanobis distance is larger than this value. double twist_gate_dist_; + /* process noise standard deviation */ + double proc_stddev_yaw_c_; //!< @brief yaw process noise + double proc_stddev_yaw_bias_c_; //!< @brief yaw bias process noise + double proc_stddev_vx_c_; //!< @brief vx process noise + double proc_stddev_wz_c_; //!< @brief wz process noise + /* process noise variance for discrete model */ double proc_cov_yaw_d_; //!< @brief discrete yaw process noise double proc_cov_yaw_bias_d_; //!< @brief discrete yaw bias process noise @@ -169,6 +180,18 @@ class EKFLocalizer : public rclcpp::Node */ void initEKF(); + /** + * @brief calculate timer rate + * @param window_size calculate the average rate of this size + * @return average timer rate + */ + double calcTimerRate(size_t window_size) const; + + /** + * @brief update predict frequency because timer rate will be /clock rate when using /clock. + */ + void updatePredictFrequency(); + /** * @brief compute EKF prediction */ diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp index 08b0f4b4f33b3..f6824920bbc95 100644 --- a/localization/ekf_localizer/src/ekf_localizer.cpp +++ b/localization/ekf_localizer/src/ekf_localizer.cpp @@ -33,7 +33,9 @@ using std::placeholders::_1; EKFLocalizer::EKFLocalizer(const std::string & node_name, const rclcpp::NodeOptions & node_options) -: rclcpp::Node(node_name, node_options), dim_x_(6 /* x, y, yaw, yaw_bias, vx, wz */) +: rclcpp::Node(node_name, node_options), + dim_x_(6 /* x, y, yaw, yaw_bias, vx, wz */), + clock_(get_clock()) { show_debug_info_ = declare_parameter("show_debug_info", false); ekf_rate_ = declare_parameter("predict_frequency", 50.0); @@ -55,30 +57,29 @@ EKFLocalizer::EKFLocalizer(const std::string & node_name, const rclcpp::NodeOpti twist_gate_dist_ = declare_parameter("twist_gate_dist", 10000.0); // Mahalanobis limit /* process noise */ - double proc_stddev_yaw_c, proc_stddev_yaw_bias_c, proc_stddev_vx_c, proc_stddev_wz_c; - proc_stddev_yaw_c = declare_parameter("proc_stddev_yaw_c", 0.005); - proc_stddev_yaw_bias_c = declare_parameter("proc_stddev_yaw_bias_c", 0.001); - proc_stddev_vx_c = declare_parameter("proc_stddev_vx_c", 5.0); - proc_stddev_wz_c = declare_parameter("proc_stddev_wz_c", 1.0); + proc_stddev_yaw_c_ = declare_parameter("proc_stddev_yaw_c", 0.005); + proc_stddev_yaw_bias_c_ = declare_parameter("proc_stddev_yaw_bias_c", 0.001); + proc_stddev_vx_c_ = declare_parameter("proc_stddev_vx_c", 5.0); + proc_stddev_wz_c_ = declare_parameter("proc_stddev_wz_c", 1.0); if (!enable_yaw_bias_estimation_) { - proc_stddev_yaw_bias_c = 0.0; + proc_stddev_yaw_bias_c_ = 0.0; } /* convert to continuous to discrete */ - proc_cov_vx_d_ = std::pow(proc_stddev_vx_c * ekf_dt_, 2.0); - proc_cov_wz_d_ = std::pow(proc_stddev_wz_c * ekf_dt_, 2.0); - proc_cov_yaw_d_ = std::pow(proc_stddev_yaw_c * ekf_dt_, 2.0); - proc_cov_yaw_bias_d_ = std::pow(proc_stddev_yaw_bias_c * ekf_dt_, 2.0); + proc_cov_vx_d_ = std::pow(proc_stddev_vx_c_ * ekf_dt_, 2.0); + proc_cov_wz_d_ = std::pow(proc_stddev_wz_c_ * ekf_dt_, 2.0); + proc_cov_yaw_d_ = std::pow(proc_stddev_yaw_c_ * ekf_dt_, 2.0); + proc_cov_yaw_bias_d_ = std::pow(proc_stddev_yaw_bias_c_ * ekf_dt_, 2.0); /* initialize ros system */ auto period_control_ns = std::chrono::duration_cast(std::chrono::duration(ekf_dt_)); timer_control_ = rclcpp::create_timer( - this, get_clock(), period_control_ns, std::bind(&EKFLocalizer::timerCallback, this)); + this, clock_, period_control_ns, std::bind(&EKFLocalizer::timerCallback, this)); const auto period_tf_ns = rclcpp::Rate(tf_rate_).period(); timer_tf_ = rclcpp::create_timer( - this, get_clock(), period_tf_ns, std::bind(&EKFLocalizer::timerTFCallback, this)); + this, clock_, period_tf_ns, std::bind(&EKFLocalizer::timerTFCallback, this)); pub_pose_ = create_publisher("ekf_pose", 1); pub_pose_cov_ = @@ -111,6 +112,47 @@ EKFLocalizer::EKFLocalizer(const std::string & node_name, const rclcpp::NodeOpti pub_measured_pose_ = create_publisher("debug/measured_pose", 1); } +/* + * calcTimerRate + */ +double EKFLocalizer::calcTimerRate(size_t window_size) const +{ + if (time_buffer_.size() < window_size) { + return ekf_rate_; + } + + const auto time_diff = (time_buffer_.back() - time_buffer_.front()).seconds(); + const auto num_intervals = time_buffer_.size() - 1; + + return static_cast(num_intervals) / time_diff; +} + +/* + * updatePredictFrequency + */ +void EKFLocalizer::updatePredictFrequency() +{ + size_t window_size = 10; + time_buffer_.push_back(clock_->now()); + while (static_cast(time_buffer_.size()) > window_size) { + time_buffer_.pop_front(); + } + + // Calc timer rate + double timer_rate = calcTimerRate(window_size); + if (timer_rate < ekf_rate_ * 0.99 || timer_rate > ekf_rate_ * 1.01) { + ekf_rate_ = timer_rate; + DEBUG_INFO(get_logger(), "[EKF] update ekf_rate_ to %f hz", ekf_rate_); + ekf_dt_ = 1.0 / std::max(ekf_rate_, 0.1); + + /* Update discrete proc_cov*/ + proc_cov_vx_d_ = std::pow(proc_stddev_vx_c_ * ekf_dt_, 2.0); + proc_cov_wz_d_ = std::pow(proc_stddev_wz_c_ * ekf_dt_, 2.0); + proc_cov_yaw_d_ = std::pow(proc_stddev_yaw_c_ * ekf_dt_, 2.0); + proc_cov_yaw_bias_d_ = std::pow(proc_stddev_yaw_bias_c_ * ekf_dt_, 2.0); + } +} + /* * timerCallback */ @@ -118,6 +160,9 @@ void EKFLocalizer::timerCallback() { DEBUG_INFO(get_logger(), "========================= timer called ========================="); + /* update predict frequency with measured timer rate */ + updatePredictFrequency(); + /* predict model in EKF */ stop_watch_.tic(); DEBUG_INFO(get_logger(), "------------------------- start prediction -------------------------"); @@ -412,7 +457,7 @@ void EKFLocalizer::measurementUpdatePose(const geometry_msgs::msg::PoseStamped & { if (pose.header.frame_id != pose_frame_id_) { RCLCPP_WARN_THROTTLE( - get_logger(), *get_clock(), std::chrono::milliseconds(2000).count(), + get_logger(), *clock_, std::chrono::milliseconds(2000).count(), "pose frame_id is %s, but pose_frame is set as %s. They must be same.", pose.header.frame_id.c_str(), pose_frame_id_.c_str()); } @@ -428,13 +473,13 @@ void EKFLocalizer::measurementUpdatePose(const geometry_msgs::msg::PoseStamped & if (delay_time < 0.0) { delay_time = 0.0; RCLCPP_WARN_THROTTLE( - get_logger(), *get_clock(), std::chrono::milliseconds(1000).count(), + get_logger(), *clock_, std::chrono::milliseconds(1000).count(), "Pose time stamp is inappropriate, set delay to 0[s]. delay = %f", delay_time); } int delay_step = std::roundf(delay_time / ekf_dt_); if (delay_step > extend_state_step_ - 1) { RCLCPP_WARN_THROTTLE( - get_logger(), *get_clock(), std::chrono::milliseconds(1000).count(), + get_logger(), *clock_, std::chrono::milliseconds(1000).count(), "Pose delay exceeds the compensation limit, ignored. delay: %f[s], limit = " "extend_state_step * ekf_dt : %f [s]", delay_time, extend_state_step_ * ekf_dt_); @@ -468,7 +513,7 @@ void EKFLocalizer::measurementUpdatePose(const geometry_msgs::msg::PoseStamped & P_y = P_curr.block(0, 0, dim_y, dim_y); if (!mahalanobisGate(pose_gate_dist_, y_ekf, y, P_y)) { RCLCPP_WARN_THROTTLE( - get_logger(), *get_clock(), std::chrono::milliseconds(2000).count(), + get_logger(), *clock_, std::chrono::milliseconds(2000).count(), "[EKF] Pose measurement update, mahalanobis distance is over limit. ignore " "measurement data."); return; @@ -516,7 +561,7 @@ void EKFLocalizer::measurementUpdateTwist(const geometry_msgs::msg::TwistStamped { if (twist.header.frame_id != "base_link") { RCLCPP_WARN_THROTTLE( - get_logger(), *get_clock(), std::chrono::milliseconds(2000).count(), + get_logger(), *clock_, std::chrono::milliseconds(2000).count(), "twist frame_id must be base_link"); } @@ -531,14 +576,14 @@ void EKFLocalizer::measurementUpdateTwist(const geometry_msgs::msg::TwistStamped double delay_time = (t_curr - twist.header.stamp).seconds() + twist_additional_delay_; if (delay_time < 0.0) { RCLCPP_WARN_THROTTLE( - get_logger(), *get_clock(), std::chrono::milliseconds(1000).count(), + get_logger(), *clock_, std::chrono::milliseconds(1000).count(), "Twist time stamp is inappropriate (delay = %f [s]), set delay to 0[s].", delay_time); delay_time = 0.0; } int delay_step = std::roundf(delay_time / ekf_dt_); if (delay_step > extend_state_step_ - 1) { RCLCPP_WARN_THROTTLE( - get_logger(), *get_clock(), std::chrono::milliseconds(1000).count(), + get_logger(), *clock_, std::chrono::milliseconds(1000).count(), "Twist delay exceeds the compensation limit, ignored. delay: %f[s], limit = " "extend_state_step * ekf_dt : %f [s]", delay_time, extend_state_step_ * ekf_dt_); @@ -566,7 +611,7 @@ void EKFLocalizer::measurementUpdateTwist(const geometry_msgs::msg::TwistStamped P_y = P_curr.block(4, 4, dim_y, dim_y); if (!mahalanobisGate(twist_gate_dist_, y_ekf, y, P_y)) { RCLCPP_WARN_THROTTLE( - get_logger(), *get_clock(), std::chrono::milliseconds(2000).count(), + get_logger(), *clock_, std::chrono::milliseconds(2000).count(), "[EKF] Twist measurement update, mahalanobis distance is over limit. ignore " "measurement data."); return; From cd797c208070371272d9df54f5e9b047728d9115 Mon Sep 17 00:00:00 2001 From: kosuke55 Date: Thu, 17 Feb 2022 13:54:23 +0900 Subject: [PATCH 2/4] Remove timer smoothing Signed-off-by: kosuke55 --- .../include/ekf_localizer/ekf_localizer.hpp | 14 +++------- .../ekf_localizer/src/ekf_localizer.cpp | 28 ++----------------- 2 files changed, 7 insertions(+), 35 deletions(-) diff --git a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp index 743a696040a75..7cb8d3a30cf35 100644 --- a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp @@ -81,8 +81,9 @@ class EKFLocalizer : public rclcpp::Node rclcpp::Clock::SharedPtr clock_; //!< @brief time for ekf calculation callback rclcpp::TimerBase::SharedPtr timer_control_; - //!< @brief time buffer for updating ekf rate - std::deque time_buffer_; + //!< @brief last predict time + std::shared_ptr last_predict_time_; + //!< @brief timer to send transform rclcpp::TimerBase::SharedPtr timer_tf_; //!< @brief tf broadcaster @@ -181,14 +182,7 @@ class EKFLocalizer : public rclcpp::Node void initEKF(); /** - * @brief calculate timer rate - * @param window_size calculate the average rate of this size - * @return average timer rate - */ - double calcTimerRate(size_t window_size) const; - - /** - * @brief update predict frequency because timer rate will be /clock rate when using /clock. + * @brief update predict frequency */ void updatePredictFrequency(); diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp index f6824920bbc95..b0f7c913c80f6 100644 --- a/localization/ekf_localizer/src/ekf_localizer.cpp +++ b/localization/ekf_localizer/src/ekf_localizer.cpp @@ -112,36 +112,13 @@ EKFLocalizer::EKFLocalizer(const std::string & node_name, const rclcpp::NodeOpti pub_measured_pose_ = create_publisher("debug/measured_pose", 1); } -/* - * calcTimerRate - */ -double EKFLocalizer::calcTimerRate(size_t window_size) const -{ - if (time_buffer_.size() < window_size) { - return ekf_rate_; - } - - const auto time_diff = (time_buffer_.back() - time_buffer_.front()).seconds(); - const auto num_intervals = time_buffer_.size() - 1; - - return static_cast(num_intervals) / time_diff; -} - /* * updatePredictFrequency */ void EKFLocalizer::updatePredictFrequency() { - size_t window_size = 10; - time_buffer_.push_back(clock_->now()); - while (static_cast(time_buffer_.size()) > window_size) { - time_buffer_.pop_front(); - } - - // Calc timer rate - double timer_rate = calcTimerRate(window_size); - if (timer_rate < ekf_rate_ * 0.99 || timer_rate > ekf_rate_ * 1.01) { - ekf_rate_ = timer_rate; + if (last_predict_time_) { + ekf_rate_ = 1.0 / (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); @@ -151,6 +128,7 @@ void EKFLocalizer::updatePredictFrequency() proc_cov_yaw_d_ = std::pow(proc_stddev_yaw_c_ * ekf_dt_, 2.0); proc_cov_yaw_bias_d_ = std::pow(proc_stddev_yaw_bias_c_ * ekf_dt_, 2.0); } + last_predict_time_ = std::make_shared(clock_->now()); } /* From abc283bad813a628380dee799a7c40d860ae4e81 Mon Sep 17 00:00:00 2001 From: kosuke55 Date: Sat, 19 Feb 2022 13:42:12 +0900 Subject: [PATCH 3/4] Remove deque Signed-off-by: kosuke55 --- .../ekf_localizer/include/ekf_localizer/ekf_localizer.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp index 7cb8d3a30cf35..0ef3f2d006712 100644 --- a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp @@ -37,7 +37,6 @@ #include #include -#include #include #include #include From ddc2f386b5afdcd391c08c708686be5623799332 Mon Sep 17 00:00:00 2001 From: kosuke55 Date: Sat, 19 Feb 2022 13:45:13 +0900 Subject: [PATCH 4/4] Remove clock_ Signed-off-by: kosuke55 --- .../include/ekf_localizer/ekf_localizer.hpp | 2 -- .../ekf_localizer/src/ekf_localizer.cpp | 28 +++++++++---------- 2 files changed, 13 insertions(+), 17 deletions(-) diff --git a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp index 0ef3f2d006712..c813369c67b77 100644 --- a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp @@ -76,8 +76,6 @@ class EKFLocalizer : public rclcpp::Node //!< @brief measurement twist with covariance subscriber rclcpp::Subscription::SharedPtr sub_twist_with_cov_; - //!< @brief clock - rclcpp::Clock::SharedPtr clock_; //!< @brief time for ekf calculation callback rclcpp::TimerBase::SharedPtr timer_control_; //!< @brief last predict time diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp index b0f7c913c80f6..9c5cb34eab8d6 100644 --- a/localization/ekf_localizer/src/ekf_localizer.cpp +++ b/localization/ekf_localizer/src/ekf_localizer.cpp @@ -33,9 +33,7 @@ using std::placeholders::_1; EKFLocalizer::EKFLocalizer(const std::string & node_name, const rclcpp::NodeOptions & node_options) -: rclcpp::Node(node_name, node_options), - dim_x_(6 /* x, y, yaw, yaw_bias, vx, wz */), - clock_(get_clock()) +: rclcpp::Node(node_name, node_options), dim_x_(6 /* x, y, yaw, yaw_bias, vx, wz */) { show_debug_info_ = declare_parameter("show_debug_info", false); ekf_rate_ = declare_parameter("predict_frequency", 50.0); @@ -75,11 +73,11 @@ EKFLocalizer::EKFLocalizer(const std::string & node_name, const rclcpp::NodeOpti auto period_control_ns = std::chrono::duration_cast(std::chrono::duration(ekf_dt_)); timer_control_ = rclcpp::create_timer( - this, clock_, period_control_ns, std::bind(&EKFLocalizer::timerCallback, this)); + this, get_clock(), period_control_ns, std::bind(&EKFLocalizer::timerCallback, this)); const auto period_tf_ns = rclcpp::Rate(tf_rate_).period(); timer_tf_ = rclcpp::create_timer( - this, clock_, period_tf_ns, std::bind(&EKFLocalizer::timerTFCallback, this)); + this, get_clock(), period_tf_ns, std::bind(&EKFLocalizer::timerTFCallback, this)); pub_pose_ = create_publisher("ekf_pose", 1); pub_pose_cov_ = @@ -118,7 +116,7 @@ EKFLocalizer::EKFLocalizer(const std::string & node_name, const rclcpp::NodeOpti void EKFLocalizer::updatePredictFrequency() { if (last_predict_time_) { - ekf_rate_ = 1.0 / (clock_->now() - *last_predict_time_).seconds(); + 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); @@ -128,7 +126,7 @@ void EKFLocalizer::updatePredictFrequency() proc_cov_yaw_d_ = std::pow(proc_stddev_yaw_c_ * ekf_dt_, 2.0); proc_cov_yaw_bias_d_ = std::pow(proc_stddev_yaw_bias_c_ * ekf_dt_, 2.0); } - last_predict_time_ = std::make_shared(clock_->now()); + last_predict_time_ = std::make_shared(get_clock()->now()); } /* @@ -435,7 +433,7 @@ void EKFLocalizer::measurementUpdatePose(const geometry_msgs::msg::PoseStamped & { if (pose.header.frame_id != pose_frame_id_) { RCLCPP_WARN_THROTTLE( - get_logger(), *clock_, std::chrono::milliseconds(2000).count(), + get_logger(), *get_clock(), std::chrono::milliseconds(2000).count(), "pose frame_id is %s, but pose_frame is set as %s. They must be same.", pose.header.frame_id.c_str(), pose_frame_id_.c_str()); } @@ -451,13 +449,13 @@ void EKFLocalizer::measurementUpdatePose(const geometry_msgs::msg::PoseStamped & if (delay_time < 0.0) { delay_time = 0.0; RCLCPP_WARN_THROTTLE( - get_logger(), *clock_, std::chrono::milliseconds(1000).count(), + get_logger(), *get_clock(), std::chrono::milliseconds(1000).count(), "Pose time stamp is inappropriate, set delay to 0[s]. delay = %f", delay_time); } int delay_step = std::roundf(delay_time / ekf_dt_); if (delay_step > extend_state_step_ - 1) { RCLCPP_WARN_THROTTLE( - get_logger(), *clock_, std::chrono::milliseconds(1000).count(), + get_logger(), *get_clock(), std::chrono::milliseconds(1000).count(), "Pose delay exceeds the compensation limit, ignored. delay: %f[s], limit = " "extend_state_step * ekf_dt : %f [s]", delay_time, extend_state_step_ * ekf_dt_); @@ -491,7 +489,7 @@ void EKFLocalizer::measurementUpdatePose(const geometry_msgs::msg::PoseStamped & P_y = P_curr.block(0, 0, dim_y, dim_y); if (!mahalanobisGate(pose_gate_dist_, y_ekf, y, P_y)) { RCLCPP_WARN_THROTTLE( - get_logger(), *clock_, std::chrono::milliseconds(2000).count(), + get_logger(), *get_clock(), std::chrono::milliseconds(2000).count(), "[EKF] Pose measurement update, mahalanobis distance is over limit. ignore " "measurement data."); return; @@ -539,7 +537,7 @@ void EKFLocalizer::measurementUpdateTwist(const geometry_msgs::msg::TwistStamped { if (twist.header.frame_id != "base_link") { RCLCPP_WARN_THROTTLE( - get_logger(), *clock_, std::chrono::milliseconds(2000).count(), + get_logger(), *get_clock(), std::chrono::milliseconds(2000).count(), "twist frame_id must be base_link"); } @@ -554,14 +552,14 @@ void EKFLocalizer::measurementUpdateTwist(const geometry_msgs::msg::TwistStamped double delay_time = (t_curr - twist.header.stamp).seconds() + twist_additional_delay_; if (delay_time < 0.0) { RCLCPP_WARN_THROTTLE( - get_logger(), *clock_, std::chrono::milliseconds(1000).count(), + get_logger(), *get_clock(), std::chrono::milliseconds(1000).count(), "Twist time stamp is inappropriate (delay = %f [s]), set delay to 0[s].", delay_time); delay_time = 0.0; } int delay_step = std::roundf(delay_time / ekf_dt_); if (delay_step > extend_state_step_ - 1) { RCLCPP_WARN_THROTTLE( - get_logger(), *clock_, std::chrono::milliseconds(1000).count(), + get_logger(), *get_clock(), std::chrono::milliseconds(1000).count(), "Twist delay exceeds the compensation limit, ignored. delay: %f[s], limit = " "extend_state_step * ekf_dt : %f [s]", delay_time, extend_state_step_ * ekf_dt_); @@ -589,7 +587,7 @@ void EKFLocalizer::measurementUpdateTwist(const geometry_msgs::msg::TwistStamped P_y = P_curr.block(4, 4, dim_y, dim_y); if (!mahalanobisGate(twist_gate_dist_, y_ekf, y, P_y)) { RCLCPP_WARN_THROTTLE( - get_logger(), *clock_, std::chrono::milliseconds(2000).count(), + get_logger(), *get_clock(), std::chrono::milliseconds(2000).count(), "[EKF] Twist measurement update, mahalanobis distance is over limit. ignore " "measurement data."); return;