Show dataset replay progress percent

MOLAorg · Jan 9, 2024 · df792f8 · df792f8
1 parent e153479
commit df792f8
Show file tree

Hide file tree

Showing 4 changed files with 28 additions and 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
@@ -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
@@ -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
@@ -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
@@ -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 (;;)