From 41dc846c38638d2df73fff6458ede31a8b5246ec Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Tue, 8 Sep 2020 15:17:46 -0700 Subject: [PATCH 1/3] Modify path tracer to use an RTF value Signed-off-by: Nate Koenig --- subt_ign/src/path_tracer.cc | 32 +++++++++++++++++++++++--------- subt_ign/src/path_tracer.hh | 7 ++++--- 2 files changed, 27 insertions(+), 12 deletions(-) diff --git a/subt_ign/src/path_tracer.cc b/subt_ign/src/path_tracer.cc index 34ed7767..9189bf7b 100644 --- a/subt_ign/src/path_tracer.cc +++ b/subt_ign/src/path_tracer.cc @@ -17,8 +17,8 @@ #include "path_tracer.hh" ////////////////////////////////////////////////// -Processor::Processor(const std::string &_path, int _stepSleepMs) - : stepSleepMs(_stepSleepMs) +Processor::Processor(const std::string &_path, double _rtf) + : rtf(_rtf) { // Create the transport node with the partition used by simulation // world. @@ -200,13 +200,21 @@ void Processor::ArtifactCb(const ignition::msgs::Pose_V &_msg) ////////////////////////////////////////////////// void Processor::DisplayPoses() { - for (auto &stepData : this->logData) + for (std::map>>::iterator iter = + this->logData.begin(); iter != this->logData.end(); ++iter) { - for (std::unique_ptr &data : stepData.second) + for (std::unique_ptr &data : iter->second) { data->Render(this); } - std::this_thread::sleep_for(std::chrono::milliseconds(this->stepSleepMs)); + + // Get the next time stamp, and sleep the correct amount of time. + auto next = std::next(iter, 1); + if (next != this->logData.end()) + { + int sleepTime = ((next->first - iter->first) / this->rtf)*1000; + std::this_thread::sleep_for(std::chrono::milliseconds(sleepTime)); + } } } @@ -337,20 +345,26 @@ void ReportData::Render(Processor *_p) ///////////////////////////////////////////////// int main(int _argc, char **_argv) { - int sleep = 20; + double rtf = 1; if (_argc > 2) { try { - sleep = std::stoi(_argv[2]); + rtf = std::stod(_argv[2]); } catch(...) { - std::cerr << "Invalid sleep time. Defaulting to 20ms\n"; + std::cerr << "Invalid RTF. Defaulting to 1.0\n"; } } + if (rtf <= 0) + { + std::cerr << "Invalid RTF of [" << rtf << "]. Defaulting to 1.0\n"; + rtf = 1.0; + } + // Create and run the processor. - Processor p(_argv[1], sleep); + Processor p(_argv[1], rtf); return 0; } diff --git a/subt_ign/src/path_tracer.hh b/subt_ign/src/path_tracer.hh index c6a9e2bc..c8ddc312 100644 --- a/subt_ign/src/path_tracer.hh +++ b/subt_ign/src/path_tracer.hh @@ -90,7 +90,8 @@ class Processor /// \brief Constructor, which also kicks off all of the data /// visualization. /// \param[in] _path Path to the directory containing the log files. - public: Processor(const std::string &_path, int _stepSleepMs); + /// \param[in] _rtf Real time factor for playback. + public: Processor(const std::string &_path, double _rtf); /// \brief Destructor public: ~Processor(); @@ -174,6 +175,6 @@ class Processor /// \brief All of the pose data. private: std::map>> logData; - /// \brief Number of milliseconds to sleep between log steps. - private: int stepSleepMs = 20; + /// \brief Realtime factor for playback. + private: double rtf = 1.0; }; From 9f30c446d3c7ce2802fdc0da6579ab0a0bf39d47 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Wed, 16 Sep 2020 10:41:54 -0700 Subject: [PATCH 2/3] Added more output Signed-off-by: Nate Koenig --- subt_ign/src/path_tracer.cc | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/subt_ign/src/path_tracer.cc b/subt_ign/src/path_tracer.cc index 9189bf7b..32f0bee9 100644 --- a/subt_ign/src/path_tracer.cc +++ b/subt_ign/src/path_tracer.cc @@ -203,6 +203,10 @@ void Processor::DisplayPoses() for (std::map>>::iterator iter = this->logData.begin(); iter != this->logData.end(); ++iter) { + printf("\r %ds/%ds (%06.2f%%)", iter->first, this->logData.rbegin()->first, + static_cast(iter->first) / this->logData.rbegin()->first * 100); + fflush(stdout); + for (std::unique_ptr &data : iter->second) { data->Render(this); @@ -366,5 +370,6 @@ int main(int _argc, char **_argv) // Create and run the processor. Processor p(_argv[1], rtf); + std::cout << "\nPlayback complete.\n"; return 0; } From b4adf4529d18f53a3793cc73984bc16dc22540a6 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Wed, 16 Sep 2020 15:09:44 -0700 Subject: [PATCH 3/3] Account for time required to process the step Signed-off-by: Nate Koenig --- subt_ign/src/path_tracer.cc | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/subt_ign/src/path_tracer.cc b/subt_ign/src/path_tracer.cc index 6004668b..2860f0a7 100644 --- a/subt_ign/src/path_tracer.cc +++ b/subt_ign/src/path_tracer.cc @@ -238,6 +238,7 @@ void Processor::DisplayPoses() for (std::map>>::iterator iter = this->logData.begin(); iter != this->logData.end(); ++iter) { + auto start = std::chrono::steady_clock::now(); printf("\r %ds/%ds (%06.2f%%)", iter->first, this->logData.rbegin()->first, static_cast(iter->first) / this->logData.rbegin()->first * 100); fflush(stdout); @@ -251,8 +252,11 @@ void Processor::DisplayPoses() auto next = std::next(iter, 1); if (next != this->logData.end()) { - int sleepTime = ((next->first - iter->first) / this->rtf)*1000; - std::this_thread::sleep_for(std::chrono::milliseconds(sleepTime)); + int sleepTime = (((next->first - iter->first) / this->rtf)*1000); + auto duration = std::chrono::steady_clock::now() - start; + std::this_thread::sleep_for(std::chrono::milliseconds(sleepTime) - + std::chrono::duration_cast( + duration)); } } }