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 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(dataset_cur_idx_), + static_cast(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(rawlog_next_idx_), + static_cast(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(rosbag_next_idx_), + static_cast(bagMessageCount_), + (100.0 * rosbag_next_idx_) / bagMessageCount_); + } // Publish observations up to current time: for (;;)