Merge pull request rpng#413 from borongyuan/master

fix erase of old imu messages
wang-lq · Jan 21, 2024 · 17b73cf · 17b73cf
2 parents e84c8ca + 60ac071
commit 17b73cf
Showing 1 changed file with 1 addition and 1 deletion.
diff --git a/ov_init/src/init/InertialInitializer.cpp b/ov_init/src/init/InertialInitializer.cpp
@@ -61,7 +61,7 @@ void InertialInitializer::feed_imu(const ov_core::ImuData &message, double oldes
   if (oldest_time != -1) {
     auto it0 = imu_data->begin();
     while (it0 != imu_data->end()) {
-      if (message.timestamp < oldest_time) {
+      if (it0->timestamp < oldest_time) {
         it0 = imu_data->erase(it0);
       } else {
         it0++;