fix(localization_util): fixed rejection criteria of SmartPoseBuffer::interpolate

localization/localization_util/src/smart_pose_buffer.cpp

@@ -33,16 +33,22 @@
 
     if (pose_buffer_.size() < 2) {
       RCLCPP_INFO(logger_, "pose_buffer_.size() < 2");
       return std::nullopt;
     }
 
     const rclcpp::Time time_first = pose_buffer_.front()->header.stamp;
     const rclcpp::Time time_last = pose_buffer_.back()->header.stamp;
 
-    if (target_ros_time < time_first || time_last < target_ros_time) {
+    if (target_ros_time < time_first) {
       return std::nullopt;
     }
 
+    // [time_last < target_ros_time] is acceptable here.
+    // It is possible that the target_ros_time (often sensor timestamp) is newer than the latest
+    // timestamp of buffered pose (often EKF).
+    // However, if the timestamp difference is too large,
+    // it will later be rejected by validate_time_stamp_difference.
+
     // get the nearest poses
     result.old_pose = *pose_buffer_.front();
     for (const PoseWithCovarianceStamped::ConstSharedPtr & pose_cov_msg_ptr : pose_buffer_) {
@@ -68,7 +74,7 @@
 
   // all validations must be true
   if (!(is_old_pose_valid && is_new_pose_valid && is_pose_diff_valid)) {
     return std::nullopt;
   }
 
   // interpolate the pose
@@ -92,22 +98,22 @@
     const rclcpp::Time msg_time = pose_msg_ptr->header.stamp;
     if (msg_time < end_time) {
       // Clear the buffer if timestamps are reversed to maintain chronological order
       pose_buffer_.clear();
     }
   }
   pose_buffer_.push_back(pose_msg_ptr);
 }
 
 void SmartPoseBuffer::pop_old(const rclcpp::Time & target_ros_time)
 {
   std::lock_guard<std::mutex> lock(mutex_);
   while (!pose_buffer_.empty()) {
     if (rclcpp::Time(pose_buffer_.front()->header.stamp) >= target_ros_time) {
       break;
     }
     pose_buffer_.pop_front();
   }
 }
 
 void SmartPoseBuffer::clear()
 {