diff --git a/mola_input_kitti_dataset/include/mola_input_kitti_dataset/KittiOdometryDataset.h b/mola_input_kitti_dataset/include/mola_input_kitti_dataset/KittiOdometryDataset.h index 38207667..edde5bd6 100644 --- a/mola_input_kitti_dataset/include/mola_input_kitti_dataset/KittiOdometryDataset.h +++ b/mola_input_kitti_dataset/include/mola_input_kitti_dataset/KittiOdometryDataset.h @@ -11,6 +11,7 @@ */ #pragma once +#include #include #include #include @@ -35,13 +36,15 @@ namespace mola * - `lidar`: Velodyne 3D LIDAR * - Ground truth poses * - * If the option `clouds_as_organized_points` is true (default), point cloud + * If the option `clouds_as_organized_points` is true, point cloud * are published as mrpt::obs::CObservationRotatingScan. - * Otherwise, they are published as mrpt::obs::CObservationPointCloud. + * Otherwise (default), they are published as mrpt::obs::CObservationPointCloud + * with X,Y,Z,I channels. * * \ingroup mola_input_kitti_dataset_grp */ class KittiOdometryDataset : public RawDataSourceBase, - public OfflineDatasetSource + public OfflineDatasetSource, + public Dataset_UI { DEFINE_MRPT_OBJECT(KittiOdometryDataset, mola) @@ -78,28 +81,61 @@ class KittiOdometryDataset : public RawDataSourceBase, mrpt::obs::CSensoryFrame::Ptr datasetGetObservations( size_t timestep) const override; + // Virtual interface of Dataset_UI (see docs in derived class) + size_t datasetUI_size() const override { return datasetSize(); } + size_t datasetUI_lastQueriedTimestep() const override + { + auto lck = mrpt::lockHelper(dataset_ui_mtx_); + return last_used_tim_index_; + } + double datasetUI_playback_speed() const override + { + auto lck = mrpt::lockHelper(dataset_ui_mtx_); + return time_warp_scale_; + } + void datasetUI_playback_speed(double speed) override + { + auto lck = mrpt::lockHelper(dataset_ui_mtx_); + time_warp_scale_ = speed; + } + bool datasetUI_paused() const override + { + auto lck = mrpt::lockHelper(dataset_ui_mtx_); + return paused_; + } + void datasetUI_paused(bool paused) override + { + auto lck = mrpt::lockHelper(dataset_ui_mtx_); + paused_ = paused; + } + void datasetUI_teleport(size_t timestep) override + { + auto lck = mrpt::lockHelper(dataset_ui_mtx_); + teleport_here_ = timestep; + } + /** See: * "IMLS-SLAM: scan-to-model matching based on 3D data", JE Deschaud, 2018. */ double VERTICAL_ANGLE_OFFSET = mrpt::DEG2RAD(0.205); private: - bool initialized_ = false; - std::string base_dir_; //!< base dir for "sequences/*". - std::string sequence_; //!< "00", "01", ... - bool clouds_as_organized_points_ = false; - unsigned int range_matrix_column_count_ = 2000; - unsigned int range_matrix_row_count_ = 64; - mrpt::Clock::time_point replay_begin_time_{}; - timestep_t replay_next_tim_index_{0}; - bool replay_started_{false}; - bool publish_lidar_{true}; - bool publish_ground_truth_{true}; - std::array publish_image_{{true, true, true, true}}; - double time_warp_scale_{1.0}; + bool initialized_ = false; + std::string base_dir_; //!< base dir for "sequences/*". + std::string sequence_; //!< "00", "01", ... + bool clouds_as_organized_points_ = false; + unsigned int range_matrix_column_count_ = 2000; + unsigned int range_matrix_row_count_ = 64; + timestep_t replay_next_tim_index_{0}; + bool publish_lidar_{true}; + bool publish_ground_truth_{true}; + std::array publish_image_{{true, true, true, true}}; std::array cam_intrinsics_; std::array cam_poses_; //!< wrt vehicle origin + std::optional last_play_wallclock_time_; + double last_dataset_time_ = 0; + std::array, 4> lst_image_; std::vector lst_velodyne_; mrpt::math::CMatrixDouble groundTruthPoses_; @@ -113,6 +149,12 @@ class KittiOdometryDataset : public RawDataSourceBase, double replay_time_{.0}; std::string seq_dir_; + mutable timestep_t last_used_tim_index_ = 0; + bool paused_ = false; + double time_warp_scale_ = 1.0; + std::optional teleport_here_; + mutable std::mutex dataset_ui_mtx_; + void load_img(const unsigned int cam_idx, const timestep_t step) const; void load_lidar(timestep_t step) const; diff --git a/mola_input_kitti_dataset/src/KittiOdometryDataset.cpp b/mola_input_kitti_dataset/src/KittiOdometryDataset.cpp index 08730081..b03bde00 100644 --- a/mola_input_kitti_dataset/src/KittiOdometryDataset.cpp +++ b/mola_input_kitti_dataset/src/KittiOdometryDataset.cpp @@ -101,6 +101,8 @@ void KittiOdometryDataset::initialize(const Yaml& c) // Optional params with default values: time_warp_scale_ = cfg.getOrDefault("time_warp_scale", time_warp_scale_); + paused_ = cfg.getOrDefault("start_paused", paused_); + publish_lidar_ = cfg.getOrDefault("publish_lidar", publish_lidar_); publish_ground_truth_ = @@ -299,17 +301,35 @@ void KittiOdometryDataset::spinOnce() ProfilerEntry tleg(profiler_, "spinOnce"); + const auto tNow = mrpt::Clock::now(); + // Starting time: - if (!replay_started_) - { - replay_begin_time_ = mrpt::Clock::now(); - replay_started_ = true; - } + if (!last_play_wallclock_time_) last_play_wallclock_time_ = tNow; // get current replay time: - const double t = - mrpt::system::timeDifference(replay_begin_time_, mrpt::Clock::now()) * - time_warp_scale_; + auto lckUIVars = mrpt::lockHelper(dataset_ui_mtx_); + const double time_warp_scale = time_warp_scale_; + const bool paused = paused_; + const auto teleport_here = teleport_here_; + teleport_here_.reset(); + lckUIVars.unlock(); + + double dt = mrpt::system::timeDifference(*last_play_wallclock_time_, tNow) * + time_warp_scale; + last_play_wallclock_time_ = tNow; + + // override by an special teleport order? + if (teleport_here.has_value() && *teleport_here < lst_timestamps_.size()) + { + replay_next_tim_index_ = *teleport_here; + last_dataset_time_ = lst_timestamps_[replay_next_tim_index_]; + } + else + { + if (paused) return; + // move forward replayed dataset time: + last_dataset_time_ += dt; + } if (replay_next_tim_index_ >= lst_timestamps_.size()) { @@ -331,11 +351,11 @@ void KittiOdometryDataset::spinOnce() // We have to publish all observations until "t": while (replay_next_tim_index_ < lst_timestamps_.size() && - t >= lst_timestamps_[replay_next_tim_index_]) + last_dataset_time_ >= lst_timestamps_[replay_next_tim_index_]) { MRPT_LOG_DEBUG_STREAM( "Sending observations for replay time: " - << mrpt::system::formatTimeInterval(t)); + << mrpt::system::formatTimeInterval(last_dataset_time_)); // Save one single timestamp for all observations, since they are in // theory shynchronized in the Kitti datasets: @@ -386,6 +406,12 @@ void KittiOdometryDataset::spinOnce() replay_next_tim_index_++; } + { + auto lck = mrpt::lockHelper(dataset_ui_mtx_); + last_used_tim_index_ = + replay_next_tim_index_ > 0 ? replay_next_tim_index_ - 1 : 0; + } + // Read ahead to save delays in the next iteration: if (replay_next_tim_index_ < lst_timestamps_.size()) { @@ -628,6 +654,11 @@ size_t KittiOdometryDataset::datasetSize() const mrpt::obs::CSensoryFrame::Ptr KittiOdometryDataset::datasetGetObservations( size_t timestep) const { + { + auto lck = mrpt::lockHelper(dataset_ui_mtx_); + last_used_tim_index_ = timestep; + } + auto sf = mrpt::obs::CSensoryFrame::Create(); for (size_t i = 0; i < publish_image_.size(); i++) diff --git a/mola_input_mulran_dataset/include/mola_input_mulran_dataset/MulranDataset.h b/mola_input_mulran_dataset/include/mola_input_mulran_dataset/MulranDataset.h index b6df96c4..9d1b3bcd 100644 --- a/mola_input_mulran_dataset/include/mola_input_mulran_dataset/MulranDataset.h +++ b/mola_input_mulran_dataset/include/mola_input_mulran_dataset/MulranDataset.h @@ -11,6 +11,7 @@ */ #pragma once +#include #include #include #include @@ -74,7 +75,9 @@ namespace mola * * \ingroup mola_input_mulran_dataset_grp */ -class MulranDataset : public RawDataSourceBase, public OfflineDatasetSource +class MulranDataset : public RawDataSourceBase, + public OfflineDatasetSource, + public Dataset_UI { DEFINE_MRPT_OBJECT(MulranDataset, mola) @@ -112,17 +115,50 @@ class MulranDataset : public RawDataSourceBase, public OfflineDatasetSource bool hasGPS() const { return !gpsCsvData_.empty(); } + // Virtual interface of Dataset_UI (see docs in derived class) + size_t datasetUI_size() const override { return datasetSize(); } + size_t datasetUI_lastQueriedTimestep() const override + { + auto lck = mrpt::lockHelper(dataset_ui_mtx_); + return last_used_tim_index_; + } + double datasetUI_playback_speed() const override + { + auto lck = mrpt::lockHelper(dataset_ui_mtx_); + return time_warp_scale_; + } + void datasetUI_playback_speed(double speed) override + { + auto lck = mrpt::lockHelper(dataset_ui_mtx_); + time_warp_scale_ = speed; + } + bool datasetUI_paused() const override + { + auto lck = mrpt::lockHelper(dataset_ui_mtx_); + return paused_; + } + void datasetUI_paused(bool paused) override + { + auto lck = mrpt::lockHelper(dataset_ui_mtx_); + paused_ = paused; + } + void datasetUI_teleport(size_t timestep) override + { + auto lck = mrpt::lockHelper(dataset_ui_mtx_); + teleport_here_ = timestep; + } + private: - bool initialized_ = false; - std::string base_dir_; //!< base dir for "sequences/*". - std::string sequence_; //!< "00", "01", ... - bool lidar_to_ground_truth_1to1_ = true; - mrpt::Clock::time_point replay_begin_time_{}; - bool replay_started_{false}; - bool publish_lidar_{true}; - bool publish_gps_{true}; - bool publish_ground_truth_{true}; - double time_warp_scale_{1.0}; + bool initialized_ = false; + std::string base_dir_; //!< base dir for "sequences/*". + std::string sequence_; //!< "00", "01", ... + bool lidar_to_ground_truth_1to1_ = true; + bool publish_lidar_{true}; + bool publish_gps_{true}; + bool publish_ground_truth_{true}; + + std::optional last_play_wallclock_time_; + double last_dataset_time_ = 0; enum class EntryType : uint8_t { @@ -166,6 +202,12 @@ class MulranDataset : public RawDataSourceBase, public OfflineDatasetSource void autoUnloadOldEntries() const; static double LidarFileNameToTimestamp(const std::string& filename); + + mutable timestep_t last_used_tim_index_ = 0; + bool paused_ = false; + double time_warp_scale_ = 1.0; + std::optional teleport_here_; + mutable std::mutex dataset_ui_mtx_; }; } // namespace mola diff --git a/mola_input_mulran_dataset/src/MulranDataset.cpp b/mola_input_mulran_dataset/src/MulranDataset.cpp index 38bcd93c..3dea7421 100644 --- a/mola_input_mulran_dataset/src/MulranDataset.cpp +++ b/mola_input_mulran_dataset/src/MulranDataset.cpp @@ -91,6 +91,8 @@ void MulranDataset::initialize(const Yaml& c) // Optional params with default values: YAML_LOAD_MEMBER_OPT(time_warp_scale, double); + paused_ = cfg.getOrDefault("start_paused", paused_); + YAML_LOAD_MEMBER_OPT(publish_lidar, bool); YAML_LOAD_MEMBER_OPT(publish_gps, bool); YAML_LOAD_MEMBER_OPT(publish_ground_truth, bool); @@ -270,17 +272,40 @@ void MulranDataset::spinOnce() ProfilerEntry tleg(profiler_, "spinOnce"); + const auto tNow = mrpt::Clock::now(); + // Starting time: - if (!replay_started_) - { - replay_begin_time_ = mrpt::Clock::now(); - replay_started_ = true; - } + if (!last_play_wallclock_time_) last_play_wallclock_time_ = tNow; // get current replay time: - const double t = - mrpt::system::timeDifference(replay_begin_time_, mrpt::Clock::now()) * - time_warp_scale_; + auto lckUIVars = mrpt::lockHelper(dataset_ui_mtx_); + const double time_warp_scale = time_warp_scale_; + const bool paused = paused_; + const auto teleport_here = teleport_here_; + teleport_here_.reset(); + lckUIVars.unlock(); + + double dt = mrpt::system::timeDifference(*last_play_wallclock_time_, tNow) * + time_warp_scale; + last_play_wallclock_time_ = tNow; + + const double t0 = datasetEntries_.begin()->first; + + // override by an special teleport order? + if (teleport_here.has_value() && *teleport_here < datasetEntries_.size()) + { + auto it = datasetEntries_.begin(); + std::advance(it, *teleport_here); + + replay_next_it_ = it; + last_dataset_time_ = it->first - t0; + } + else + { + if (paused) return; + // move forward replayed dataset time: + last_dataset_time_ += dt; + } if (replay_next_it_ == datasetEntries_.end()) { @@ -303,17 +328,15 @@ void MulranDataset::spinOnce() (100.0 * pos) / (datasetEntries_.size())); } - const double t0 = datasetEntries_.begin()->first; - std::optional lastUsedLidarIdx; // We have to publish all observations until "t": while (replay_next_it_ != datasetEntries_.end() && - t >= (replay_next_it_->first - t0)) + last_dataset_time_ >= (replay_next_it_->first - t0)) { MRPT_LOG_DEBUG_STREAM( "Sending observations for replay time: " - << mrpt::system::formatTimeInterval(t)); + << mrpt::system::formatTimeInterval(last_dataset_time_)); const auto& de = replay_next_it_->second; @@ -372,6 +395,12 @@ void MulranDataset::spinOnce() replay_next_it_++; } + { + auto lck = mrpt::lockHelper(dataset_ui_mtx_); + last_used_tim_index_ = + std::distance(datasetEntries_.begin(), replay_next_it_); + } + // Read ahead to save delays in the next iteration: if (lastUsedLidarIdx) { @@ -581,6 +610,11 @@ size_t MulranDataset::datasetSize() const mrpt::obs::CSensoryFrame::Ptr MulranDataset::datasetGetObservations( size_t timestep) const { + { + auto lck = mrpt::lockHelper(dataset_ui_mtx_); + last_used_tim_index_ = timestep; + } + auto sf = mrpt::obs::CSensoryFrame::Create(); if (publish_lidar_)