Path tracer rtf

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.
@@ -235,13 +235,29 @@ void Processor::ArtifactCb(const ignition::msgs::Pose_V &_msg)
 //////////////////////////////////////////////////
 void Processor::DisplayPoses()
 {
-  for (auto &stepData : this->logData)
+  for (std::map<int, std::vector<std::unique_ptr<Data>>>::iterator iter =
+       this->logData.begin(); iter != this->logData.end(); ++iter)
   {
-    for (std::unique_ptr<Data> &data : stepData.second)
+    auto start = std::chrono::steady_clock::now();
+    printf("\r %ds/%ds (%06.2f%%)", iter->first, this->logData.rbegin()->first,
+        static_cast<double>(iter->first) / this->logData.rbegin()->first * 100);
+    fflush(stdout);
+
+    for (std::unique_ptr<Data> &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);
+      auto duration = std::chrono::steady_clock::now() - start;
+      std::this_thread::sleep_for(std::chrono::milliseconds(sleepTime)  -
+          std::chrono::duration_cast<std::chrono::nanoseconds>(
+            duration));
+    }
   }
 }
 
@@ -372,20 +388,27 @@ 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);
+  std::cout << "\nPlayback complete.\n";
   return 0;
 }

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<int, std::vector<std::unique_ptr<Data>>> logData;
 
-  /// \brief Number of milliseconds to sleep between log steps.
-  private: int stepSleepMs = 20;
+  /// \brief Realtime factor for playback.
+  private: double rtf = 1.0;
 };