Add lockstep mode to video recording

include/ignition/gazebo/rendering/RenderUtil.hh

@@ -119,6 +119,11 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE {
     /// Returns reference to the marker manager.
     public: class MarkerManager &MarkerManager();
 
+    /// \brief Get simulation time that the current rendering state corresponds
+    /// to
+    /// \returns Simulation time.
+    public: std::chrono::steady_clock::duration SimTime() const;
+
     /// \brief Set the entity being selected
     /// \param[in] _node Node representing the selected entity
     /// \TODO(anyone) Make const ref when merging forward

src/gui/plugins/scene3d/Scene3D.cc

@@ -15,8 +15,10 @@
  *
 */
 
+#include <condition_variable>
 #include <cmath>
 #include <map>
+#include <mutex>
 #include <sstream>
 #include <string>
 #include <vector>
@@ -62,6 +64,15 @@
 
 #include "Scene3D.hh"
 
+/// \brife True if rendering has been initialized
+bool g_renderInit = false;
+
+/// \brief condition variable for lockstepping video recording
+std::condition_variable g_renderCv;
+
+/// \brief mutex for lockstepping video recording
+std::mutex g_renderMutex;
+
 Q_DECLARE_METATYPE(std::string)
 
 namespace ignition
@@ -194,6 +205,10 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE {
     /// \brief Path to save the recorded video
     public: std::string recordVideoSavePath;
 
+    /// \brief Use sim time as timestamp during video recording
+    /// By default (false), video encoding is done using real time.
+    public: bool recordVideoUseSimTime = false;
+
     /// \brief Target to move the user camera to
     public: std::string moveToTarget;
 
@@ -375,6 +390,15 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE {
 
     /// \brief Camera pose publisher
     public: transport::Node::Publisher cameraPosePub;
+
+    /// \brief lockstep ECM updates with rendering
+    public: bool recordVideoLockstep = false;
+
+    /// \brief True to indicate video recording in progress
+    public: bool recording = false;
+
+    /// \brief mutex to protect the recording variable
+    public: std::mutex recordMutex;
   };
 }
 }
@@ -475,12 +499,22 @@ void IgnRenderer::Render()
       if (this->dataPtr->videoEncoder.IsEncoding())
       {
         this->dataPtr->camera->Copy(this->dataPtr->cameraImage);
+
+        std::chrono::steady_clock::time_point t =
+            std::chrono::steady_clock::now();
+        if (this->dataPtr->recordVideoUseSimTime)
+        {
+          t = std::chrono::steady_clock::time_point(
+              this->dataPtr->renderUtil.SimTime());
+        }
         this->dataPtr->videoEncoder.AddFrame(
-            this->dataPtr->cameraImage.Data<unsigned char>(), width, height);
+            this->dataPtr->cameraImage.Data<unsigned char>(), width, height, t);
       }
       // Video recorder is idle. Start recording.
       else
       {
+        if (this->dataPtr->recordVideoUseSimTime)
+          ignmsg << "Recording video using sim time." << std::endl;
         this->dataPtr->videoEncoder.Start(this->dataPtr->recordVideoFormat,
             this->dataPtr->recordVideoSavePath, width, height);
       }
@@ -672,6 +706,10 @@ void IgnRenderer::Render()
         ignition::gui::App()->findChild<ignition::gui::MainWindow *>(),
         &event);
   }
+
+  // only has an effect in video recording lockstep mode
+  // this notifes ECM to continue updating the scene
+  g_renderCv.notify_one();
 }
 
 /////////////////////////////////////////////////
@@ -1654,6 +1692,13 @@ void IgnRenderer::SetRecordVideo(bool _record, const std::string &_format,
   this->dataPtr->recordVideoSavePath = _savePath;
 }
 
+/////////////////////////////////////////////////
+void IgnRenderer::SetRecordVideoUseSimTime(bool _useSimTime)
+{
+  std::lock_guard<std::mutex> lock(this->dataPtr->mutex);
+  this->dataPtr->recordVideoUseSimTime = _useSimTime;
+}
+
 /////////////////////////////////////////////////
 void IgnRenderer::SetMoveTo(const std::string &_target)
 {
@@ -2028,6 +2073,8 @@ void RenderWindowItem::Ready()
 
   this->dataPtr->renderThread->start();
   this->update();
+
+  g_renderInit = true;
 }
 
 /////////////////////////////////////////////////
@@ -2270,6 +2317,40 @@ void Scene3D::LoadConfig(const tinyxml2::XMLElement *_pluginElem)
       }
     }
 
+    if (auto elem = _pluginElem->FirstChildElement("record_video"))
+    {
+      if (auto useSimTimeElem = elem->FirstChildElement("use_sim_time"))
+      {
+        std::string useSimTimeStr =
+            common::lowercase(useSimTimeElem->GetText());
+        if (useSimTimeStr == "true" || useSimTimeStr == "1")
+          renderWindow->SetRecordVideoUseSimTime(true);
+        else if (useSimTimeStr == "false" || useSimTimeStr == "0")
+          renderWindow->SetRecordVideoUseSimTime(false);
+        else
+        {
+          ignerr << "Faild to parse <use_sim_time> value: " << useSimTimeStr
+                 << std::endl;
+        }
+      }
+      if (auto lockstepElem = elem->FirstChildElement("lockstep"))
+      {
+        std::string lockstepStr =
+            common::lowercase(lockstepElem->GetText());
+        if (lockstepStr == "true" || lockstepStr == "1")
+        {
+          this->dataPtr->recordVideoLockstep = true;
+        }
+        else if (lockstepStr == "false" || lockstepStr == "0")
+          this->dataPtr->recordVideoLockstep = false;
+        else
+        {
+          ignerr << "Faild to parse <lockstep> value: " << lockstepStr
+                 << std::endl;
+        }
+      }
+    }
+
     if (auto elem = _pluginElem->FirstChildElement("fullscreen"))
     {
       auto fullscreen = false;
@@ -2383,6 +2464,16 @@ void Scene3D::Update(const UpdateInfo &_info,
     this->dataPtr->cameraPosePub.Publish(poseMsg);
   }
   this->dataPtr->renderUtil->UpdateFromECM(_info, _ecm);
+
+  // check if video recording is enabled and if we need to lock step
+  // ECM updates with GUI rendering during video recording
+  std::unique_lock<std::mutex> lock(this->dataPtr->recordMutex);
+  if (this->dataPtr->recording && this->dataPtr->recordVideoLockstep &&
+      g_renderInit)
+  {
+    std::unique_lock<std::mutex> lock2(g_renderMutex);
+    g_renderCv.wait(lock2);
+  }
 }
 
 /////////////////////////////////////////////////
@@ -2406,6 +2497,12 @@ bool Scene3D::OnRecordVideo(const msgs::VideoRecord &_msg,
   renderWindow->SetRecordVideo(record, _msg.format(), _msg.save_filename());
 
   _res.set_data(true);
+
+  std::unique_lock<std::mutex> lock(this->dataPtr->recordMutex);
+  this->dataPtr->recording = record;
+  if (this->dataPtr->recording && this->dataPtr->recordVideoLockstep)
+    ignmsg << "Recording video in lockstep mode" << std::endl;
+
   return true;
 }
 
@@ -2733,6 +2830,13 @@ void RenderWindowItem::SetWorldName(const std::string &_name)
   this->dataPtr->renderThread->ignRenderer.worldName = _name;
 }
 
+/////////////////////////////////////////////////
+void RenderWindowItem::SetRecordVideoUseSimTime(bool _useSimTime)
+{
+  this->dataPtr->renderThread->ignRenderer.SetRecordVideoUseSimTime(
+      _useSimTime);
+}
+
 /////////////////////////////////////////////////
 void RenderWindowItem::OnHovered(const ignition::math::Vector2i &_hoverPos)
 {

src/gui/plugins/scene3d/Scene3D.hh

@@ -206,6 +206,10 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE {
     public: void SetRecordVideo(bool _record, const std::string &_format,
         const std::string &_savePath);
 
+    /// \brief Set whether to record video using sim time as timestamp
+    /// \param[in] _true True record video using sim time
+    public: void SetRecordVideoUseSimTime(bool _useSimTime);
+
     /// \brief Move the user camera to move to the speficied target
     /// \param[in] _target Target to move the camera to
     public: void SetMoveTo(const std::string &_target);
@@ -515,6 +519,10 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE {
     public: void SetRecordVideo(bool _record, const std::string &_format,
         const std::string &_savePath);
 
+    /// \brief Set whether to record video using sim time as timestamp
+    /// \param[in] _true True record video using sim time
+    public: void SetRecordVideoUseSimTime(bool _useSimTime);
+
     /// \brief Move the user camera to move to the specified target
     /// \param[in] _target Target to move the camera to
     public: void SetMoveTo(const std::string &_target);

src/rendering/RenderUtil.cc

@@ -1143,6 +1143,13 @@ MarkerManager &RenderUtil::MarkerManager()
   return this->dataPtr->markerManager;
 }
 
+//////////////////////////////////////////////////
+std::chrono::steady_clock::duration RenderUtil::SimTime() const
+{
+  std::lock_guard<std::mutex> lock(this->dataPtr->updateMutex);
+  return this->dataPtr->simTime;
+}
+
 /////////////////////////////////////////////////
 // NOLINTNEXTLINE
 void RenderUtil::SetSelectedEntity(rendering::NodePtr _node)