From 4fcc980dba0372a122d4d189f5bc4bbf4067e516 Mon Sep 17 00:00:00 2001
From: Zygfryd Wieszok <zwieszok@autonomous-systems.pl>
Date: Tue, 8 Mar 2022 13:40:05 +0100
Subject: [PATCH] Fixed state history reversion

---
 src/ros_filter.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/ros_filter.cpp b/src/ros_filter.cpp
index 3ab8b52b7..88e61dc9c 100644
--- a/src/ros_filter.cpp
+++ b/src/ros_filter.cpp
@@ -3341,7 +3341,7 @@ bool RosFilter<T>::revertTo(const rclcpp::Time & time)
   // If we have a valid reversion state, revert
   if (last_history_state) {
     // Reset filter to the latest state from the queue.
-    const FilterStatePtr & state = filter_state_history_.back();
+    const FilterStatePtr & state = last_history_state;
     filter_.setState(state->state_);
     filter_.setEstimateErrorCovariance(state->estimate_error_covariance_);
     filter_.setLastMeasurementTime(state->last_measurement_time_);