From df792f8b6217f8dfa3f78372da8ac385a6aa9760 Mon Sep 17 00:00:00 2001
From: Jose Luis Blanco-Claraco <joseluisblancoc@gmail.com>
Date: Tue, 9 Jan 2024 12:33:24 +0100
Subject: [PATCH] Show dataset replay progress percent

---
 .../include/mola_input_euroc_dataset/EurocDataset.h  |  1 +
 mola_input_euroc_dataset/src/EurocDataset.cpp        | 12 +++++++++++-
 mola_input_rawlog/src/RawlogDataset.cpp              |  8 ++++++++
 mola_input_rosbag2/src/Rosbag2Dataset.cpp            |  8 ++++++++
 4 files changed, 28 insertions(+), 1 deletion(-)

diff --git a/mola_input_euroc_dataset/include/mola_input_euroc_dataset/EurocDataset.h b/mola_input_euroc_dataset/include/mola_input_euroc_dataset/EurocDataset.h
index 4f74e0a5..20cd4a22 100644
--- a/mola_input_euroc_dataset/include/mola_input_euroc_dataset/EurocDataset.h
+++ b/mola_input_euroc_dataset/include/mola_input_euroc_dataset/EurocDataset.h
@@ -69,6 +69,7 @@ class EurocDataset : public RawDataSourceBase
     std::array<mrpt::math::TPose3D, 2> cam_poses_;  //!< wrt vehicle origin
     euroc_dataset_t                    dataset_;  //!< dataset itself
     euroc_dataset_t::iterator          dataset_next_;  //!< next item to publish
+    size_t                             dataset_cur_idx_ = 0;
 
     // double              replay_time_{.0};
     std::string seq_dir_;
diff --git a/mola_input_euroc_dataset/src/EurocDataset.cpp b/mola_input_euroc_dataset/src/EurocDataset.cpp
index 15811f21..044e987a 100644
--- a/mola_input_euroc_dataset/src/EurocDataset.cpp
+++ b/mola_input_euroc_dataset/src/EurocDataset.cpp
@@ -211,7 +211,8 @@ void EurocDataset::initialize(const Yaml& c)
     }
 
     // Start at the dataset begin:
-    dataset_next_ = dataset_.begin();
+    dataset_next_    = dataset_.begin();
+    dataset_cur_idx_ = 0;
 
     MRPT_END
 }  // end initialize()
@@ -245,6 +246,14 @@ void EurocDataset::spinOnce()
             "End of dataset reached! Nothing else to publish (CTRL+C to quit)");
         return;
     }
+    else
+    {
+        MRPT_LOG_THROTTLE_INFO_FMT(
+            5.0, "Dataset replay progress: %lu / %lu  (%4.02f%%)",
+            static_cast<unsigned long>(dataset_cur_idx_),
+            static_cast<unsigned long>(dataset_.size()),
+            (100.0 * dataset_cur_idx_) / (dataset_.size()));
+    }
 
     // We have to publish all observations until "t":
     while (dataset_next_ != dataset_.end() && tim >= dataset_next_->first)
@@ -274,6 +283,7 @@ void EurocDataset::spinOnce()
 
         // Advance:
         ++dataset_next_;
+        ++dataset_cur_idx_;
     }
 
     // Read ahead to save delays in the next iteration:
diff --git a/mola_input_rawlog/src/RawlogDataset.cpp b/mola_input_rawlog/src/RawlogDataset.cpp
index de05ddd7..dc2707dc 100644
--- a/mola_input_rawlog/src/RawlogDataset.cpp
+++ b/mola_input_rawlog/src/RawlogDataset.cpp
@@ -110,6 +110,14 @@ void RawlogDataset::spinOnce()
             "End of dataset reached! Nothing else to publish (CTRL+C to quit)");
         return;
     }
+    else if (read_all_first_)
+    {
+        MRPT_LOG_THROTTLE_INFO_FMT(
+            5.0, "Dataset replay progress: %lu / %lu  (%4.02f%%)",
+            static_cast<unsigned long>(rawlog_next_idx_),
+            static_cast<unsigned long>(rawlog_entire_.size()),
+            (100.0 * rawlog_next_idx_) / (rawlog_entire_.size()));
+    }
 
     // First rawlog timestamp?
     if (rawlog_begin_time_ == INVALID_TIMESTAMP)
diff --git a/mola_input_rosbag2/src/Rosbag2Dataset.cpp b/mola_input_rosbag2/src/Rosbag2Dataset.cpp
index 715f57b3..896a98b3 100644
--- a/mola_input_rosbag2/src/Rosbag2Dataset.cpp
+++ b/mola_input_rosbag2/src/Rosbag2Dataset.cpp
@@ -333,6 +333,14 @@ void Rosbag2Dataset::spinOnce()
             "quit)");
         return;
     }
+    else
+    {
+        MRPT_LOG_THROTTLE_INFO_FMT(
+            5.0, "Dataset replay progress: %lu / %lu  (%4.02f%%)",
+            static_cast<unsigned long>(rosbag_next_idx_),
+            static_cast<unsigned long>(bagMessageCount_),
+            (100.0 * rosbag_next_idx_) / bagMessageCount_);
+    }
 
     // Publish observations up to current time:
     for (;;)