Add lockstep mode to video recording (#419)

Signed-off-by: Ian Chen <ichen@osrfoundation.org>
Signed-off-by: Louise Poubel <louise@openrobotics.org>

Co-authored-by: Nate Koenig <nate@openrobotics.org>
Co-authored-by: Louise Poubel <louise@openrobotics.org>

src/gui/plugins/scene3d/Scene3D.cc

@@ -19,8 +19,10 @@
 
 #include <algorithm>
 #include <cmath>
+#include <condition_variable>
 #include <limits>
 #include <map>
+#include <mutex>
 #include <sstream>
 #include <string>
 #include <utility>
@@ -66,6 +68,11 @@
 #include "ignition/gazebo/gui/GuiEvents.hh"
 #include "ignition/gazebo/rendering/RenderUtil.hh"
 
+/// \brief condition variable for lockstepping video recording
+/// todo(anyone) avoid using a global condition variable when we support
+/// multiple viewports in the future.
+std::condition_variable g_renderCv;
+
 Q_DECLARE_METATYPE(std::string)
 
 namespace ignition
@@ -202,6 +209,22 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE {
     /// By default (false), video encoding is done using real time.
     public: bool recordVideoUseSimTime = false;
 
+    /// \brief Lockstep gui with ECM when recording
+    public: bool recordVideoLockstep = false;
+
+    /// \brief Video recorder bitrate (bps)
+    public: unsigned int recordVideoBitrate = 2070000;
+
+    /// \brief Previous camera update time during video recording
+    /// only used in lockstep mode and recording in sim time.
+    public: std::chrono::steady_clock::time_point recordVideoUpdateTime;
+
+    /// \brief Start tiem of video recording
+    public: std::chrono::steady_clock::time_point recordStartTime;
+
+    /// \brief Camera pose publisher
+    public: transport::Node::Publisher recorderStatsPub;
+
     /// \brief Target to move the user camera to
     public: std::string moveToTarget;
 
@@ -344,6 +367,9 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE {
     /// \brief Render thread
     public : RenderThread *renderThread = nullptr;
 
+    //// \brief Set to true after the renderer is initialized
+    public: bool rendererInit = false;
+
     //// \brief List of threads
     public: static QList<QThread *> threads;
   };
@@ -386,6 +412,19 @@ 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;
+
+    /// \brief mutex to protect the render condition variable
+    /// Used when recording in lockstep mode.
+    public: std::mutex renderMutex;
   };
 }
 }
@@ -401,6 +440,13 @@ IgnRenderer::IgnRenderer()
   : dataPtr(new IgnRendererPrivate)
 {
   this->dataPtr->moveToHelper.initCameraPose = this->cameraPose;
+
+  // recorder stats topic
+  std::string recorderStatsTopic = "/gui/record_video/stats";
+  this->dataPtr->recorderStatsPub =
+    this->dataPtr->node.Advertise<msgs::Time>(recorderStatsTopic);
+  ignmsg << "Video recorder stats topic advertised on ["
+         << recorderStatsTopic << "]" << std::endl;
 }
 
 
@@ -469,7 +515,24 @@ void IgnRenderer::Render()
     }
   }
 
+  // check if recording is in lockstep mode and if it is using sim time
+  // if so, there is no need to update camera if sim time has not advanced
+  bool update = true;
+  if (this->dataPtr->recordVideoLockstep &&
+      this->dataPtr->recordVideoUseSimTime &&
+      this->dataPtr->videoEncoder.IsEncoding())
+  {
+    std::chrono::steady_clock::time_point t =
+        std::chrono::steady_clock::time_point(
+        this->dataPtr->renderUtil.SimTime());
+    if (t - this->dataPtr->recordVideoUpdateTime == std::chrono::seconds(0))
+      update = false;
+    else
+      this->dataPtr->recordVideoUpdateTime = t;
+  }
+
   // update and render to texture
+  if (update)
   {
     IGN_PROFILE("IgnRenderer::Render Update camera");
     this->dataPtr->camera->Update();
@@ -501,16 +564,51 @@ void IgnRenderer::Render()
           t = std::chrono::steady_clock::time_point(
               this->dataPtr->renderUtil.SimTime());
         }
-        this->dataPtr->videoEncoder.AddFrame(
+        bool frameAdded = this->dataPtr->videoEncoder.AddFrame(
             this->dataPtr->cameraImage.Data<unsigned char>(), width, height, t);
+
+        if (frameAdded)
+        {
+          // publish recorder stats
+          if (this->dataPtr->recordStartTime ==
+              std::chrono::steady_clock::time_point(
+              std::chrono::duration(std::chrono::seconds(0))))
+          {
+            // start time, i.e. time when first frame is added
+            this->dataPtr->recordStartTime = t;
+          }
+
+          std::chrono::steady_clock::duration dt;
+          dt = t - this->dataPtr->recordStartTime;
+          int64_t sec, nsec;
+          std::tie(sec, nsec) = ignition::math::durationToSecNsec(dt);
+          msgs::Time msg;
+          msg.set_sec(sec);
+          msg.set_nsec(nsec);
+          this->dataPtr->recorderStatsPub.Publish(msg);
+        }
       }
       // Video recorder is idle. Start recording.
       else
       {
         if (this->dataPtr->recordVideoUseSimTime)
           ignmsg << "Recording video using sim time." << std::endl;
+        if (this->dataPtr->recordVideoLockstep)
+        {
+          ignmsg << "Recording video in lockstep mode" << std::endl;
+          if (!this->dataPtr->recordVideoUseSimTime)
+          {
+            ignwarn << "It is recommended to set <use_sim_time> to true "
+                    << "when recording video in lockstep mode." << std::endl;
+          }
+        }
+        ignmsg << "Recording video using bitrate: "
+               << this->dataPtr->recordVideoBitrate <<  std::endl;
         this->dataPtr->videoEncoder.Start(this->dataPtr->recordVideoFormat,
-            this->dataPtr->recordVideoSavePath, width, height);
+            this->dataPtr->recordVideoSavePath, width, height, 25,
+            this->dataPtr->recordVideoBitrate);
+        this->dataPtr->recordStartTime = std::chrono::steady_clock::time_point(
+            std::chrono::duration(std::chrono::seconds(0)));
       }
     }
     else if (this->dataPtr->videoEncoder.IsEncoding())
@@ -708,6 +806,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();
 }
 
 /////////////////////////////////////////////////
@@ -1736,6 +1838,20 @@ void IgnRenderer::SetRecordVideoUseSimTime(bool _useSimTime)
   this->dataPtr->recordVideoUseSimTime = _useSimTime;
 }
 
+/////////////////////////////////////////////////
+void IgnRenderer::SetRecordVideoLockstep(bool _useSimTime)
+{
+  std::lock_guard<std::mutex> lock(this->dataPtr->mutex);
+  this->dataPtr->recordVideoLockstep = _useSimTime;
+}
+
+/////////////////////////////////////////////////
+void IgnRenderer::SetRecordVideoBitrate(unsigned int _bitrate)
+{
+  std::lock_guard<std::mutex> lock(this->dataPtr->mutex);
+  this->dataPtr->recordVideoBitrate = _bitrate;
+}
+
 /////////////////////////////////////////////////
 void IgnRenderer::SetMoveTo(const std::string &_target)
 {
@@ -2110,6 +2226,14 @@ void RenderWindowItem::Ready()
 
   this->dataPtr->renderThread->start();
   this->update();
+
+  this->dataPtr->rendererInit = true;
+}
+
+/////////////////////////////////////////////////
+bool RenderWindowItem::RendererInitialized() const
+{
+  return this->dataPtr->rendererInit;
 }
 
 /////////////////////////////////////////////////
@@ -2367,6 +2491,35 @@ void Scene3D::LoadConfig(const tinyxml2::XMLElement *_pluginElem)
           renderWindow->SetRecordVideoUseSimTime(useSimTime);
         }
       }
+      if (auto lockstepElem = elem->FirstChildElement("lockstep"))
+      {
+        bool lockstep = false;
+        if (lockstepElem->QueryBoolText(&lockstep) != tinyxml2::XML_SUCCESS)
+        {
+          ignerr << "Failed to parse <lockstep> value: "
+                 << lockstepElem->GetText() << std::endl;
+        }
+        else
+        {
+          renderWindow->SetRecordVideoLockstep(lockstep);
+        }
+      }
+      if (auto bitrateElem = elem->FirstChildElement("bitrate"))
+      {
+        unsigned int bitrate = 0u;
+        std::stringstream bitrateStr;
+        bitrateStr << std::string(bitrateElem->GetText());
+        bitrateStr >> bitrate;
+        if (bitrate > 0u)
+        {
+          renderWindow->SetRecordVideoBitrate(bitrate);
+        }
+        else
+        {
+          ignerr << "Video recorder bitrate must be larger than 0"
+                 << std::endl;
+        }
+      }
     }
 
     if (auto elem = _pluginElem->FirstChildElement("fullscreen"))
@@ -2484,6 +2637,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 &&
+      renderWindow->RendererInitialized())
+  {
+    std::unique_lock<std::mutex> lock2(this->dataPtr->renderMutex);
+    g_renderCv.wait(lock2);
+  }
 }
 
 /////////////////////////////////////////////////
@@ -2507,6 +2670,9 @@ 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;
   return true;
 }
 
@@ -2860,6 +3026,20 @@ void RenderWindowItem::SetRecordVideoUseSimTime(bool _useSimTime)
       _useSimTime);
 }
 
+/////////////////////////////////////////////////
+void RenderWindowItem::SetRecordVideoLockstep(bool _lockstep)
+{
+  this->dataPtr->renderThread->ignRenderer.SetRecordVideoLockstep(
+      _lockstep);
+}
+
+/////////////////////////////////////////////////
+void RenderWindowItem::SetRecordVideoBitrate(unsigned int _bitrate)
+{
+  this->dataPtr->renderThread->ignRenderer.SetRecordVideoBitrate(
+      _bitrate);
+}
+
 /////////////////////////////////////////////////
 void RenderWindowItem::OnHovered(const ignition::math::Vector2i &_hoverPos)
 {

src/gui/plugins/scene3d/Scene3D.hh

@@ -210,6 +210,14 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE {
     /// \param[in] _true True record video using sim time
     public: void SetRecordVideoUseSimTime(bool _useSimTime);
 
+    /// \brief Set whether to record video in lockstep mode
+    /// \param[in] _true True to record video in lockstep mode
+    public: void SetRecordVideoLockstep(bool _lockstep);
+
+    /// \brief Set video recorder bitrate in bps
+    /// \param[in] _bitrate Bit rate to set to
+    public: void SetRecordVideoBitrate(unsigned int _bitrate);
+
     /// \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);
@@ -529,6 +537,14 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE {
     /// \param[in] _true True record video using sim time
     public: void SetRecordVideoUseSimTime(bool _useSimTime);
 
+    /// \brief Set whether to record video in lockstep mode
+    /// \param[in] _true True to record video in lockstep mode
+    public: void SetRecordVideoLockstep(bool _lockstep);
+
+    /// \brief Set video recorder bitrate in bps
+    /// \param[in] _bitrate Bit rate to set to
+    public: void SetRecordVideoBitrate(unsigned int _bitrate);
+
     /// \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);
@@ -610,6 +626,12 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE {
     /// the render window.
     public: void OnHovered(const ignition::math::Vector2i &_hoverPos);
 
+    /// \brief Get whether the renderer is initialized. The renderer is
+    /// initialized when the context is created and the render thread is
+    /// started.
+    /// \return True if the renderer is initialized.
+    public: bool RendererInitialized() const;
+
     /// \brief Slot called when thread is ready to be started
     public Q_SLOTS: void Ready();
 

src/systems/scene_broadcaster/SceneBroadcaster.cc

@@ -226,6 +226,11 @@ void SceneBroadcaster::Configure(
   auto readHertz = _sdf->Get<int>("dynamic_pose_hertz", 60);
   this->dataPtr->dyPoseHertz = readHertz.first;
 
+  auto stateHerz = _sdf->Get<int>("state_hertz", 60);
+  this->dataPtr->statePublishPeriod =
+      std::chrono::duration<int64_t, std::ratio<1, 1000>>(
+      std::chrono::milliseconds(1000/stateHerz.first));
+
   // Add to graph
   {
     std::lock_guard<std::mutex> lock(this->dataPtr->graphMutex);

tutorials.md.in

@@ -22,6 +22,7 @@ Ignition @IGN_DESIGNATION_CAP@ library and how to use the library effectively.
 * \subpage detachablejoints "Detachable Joints": Creating models that start off rigidly attached and then get detached during simulation.
 * \subpage triggeredpublisher "Triggered Publisher": Using the TriggeredPublisher system to orchestrate actions in simulation.
 * \subpage logicalaudiosensor "Logical Audio Sensor": Using the LogicalAudioSensor system to mimic logical audio emission and detection in simulation.
+* \subpage videorecorder "Video Recorder": Record videos from the 3D render window.
 
 **Migration from Gazebo classic**