From a0071888114f35fb00bf3f8c1999b90c99c674bd Mon Sep 17 00:00:00 2001 From: Yuma Nihei Date: Sat, 28 Sep 2024 00:02:07 +0900 Subject: [PATCH] fix(pose_instability_detector): empty check order --- .../src/pose_instability_detector.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/localization/pose_instability_detector/src/pose_instability_detector.cpp b/localization/pose_instability_detector/src/pose_instability_detector.cpp index 02d58e7621f3c..2146bc947d6aa 100644 --- a/localization/pose_instability_detector/src/pose_instability_detector.cpp +++ b/localization/pose_instability_detector/src/pose_instability_detector.cpp @@ -88,9 +88,12 @@ void PoseInstabilityDetector::callback_odometry(Odometry::ConstSharedPtr odometr } // If twist_buffer_ is empty OR the latest twist is too old, skip the following. + if (twist_buffer_.empty()) { + return; + } double latest_twist_age = (current_pose_time - rclcpp::Time(twist_buffer_.back().header.stamp)).seconds(); - if (twist_buffer_.empty() || latest_twist_age > window_length_ * 1.5) { + if (latest_twist_age > window_length_ * 1.5) { return; }