Support locked entities, and headless video recording using sim time

include/ignition/gazebo/EntityComponentManager.hh

@@ -91,6 +91,35 @@ namespace ignition
       public: void RequestRemoveEntity(const Entity _entity,
           bool _recursive = true);
 
+      /// \brief Prevent an entity and optionally its children from
+      /// being removed.
+      ///
+      /// This function can be useful when seek operations during log
+      /// playback are used in conjunciton with spawned entities. For
+      /// example, you may want to record a video based on a log file
+      /// using a headless simulation instance. This requires a
+      /// camera sensor which would be spawned during log playback. If
+      /// a seek backward in time is performed during log playback, then the
+      /// spawned camera would be removed. Use this function to prevent the
+      /// camera from automatic removal.
+      ///
+      /// \param[in] _entity Entity to be locked.
+      /// \param[in] _recursive Whether to recursively lock all child
+      /// entities. True by default.
+      public: void LockEntity(const Entity _entity, bool _recursive = true);
+
+      /// \brief Allow a previously lock entity and optionally its children
+      /// to be removed.
+      /// \param[in] _entity Entity to be unlocked.
+      /// \param[in] _recursive Whether to recursively unlock all child
+      /// entities. True by default.
+      /// \sa void LockEntity(const Entity, bool)
+      public: void UnlockEntity(const Entity _entity, bool _recursive = true);
+
+      /// \brief Allow all previously locked entitied to be removed.
+      /// \sa void LockEntity(const Entity, bool)
+      public: void UnlockAllEntities();
+
       /// \brief Request to remove all entities. This will insert the request
       /// into a queue. The queue is processed toward the end of a simulation
       /// update step.

src/EntityComponentManager.cc

@@ -45,6 +45,13 @@ class ignition::gazebo::EntityComponentManagerPrivate
   public: void InsertEntityRecursive(Entity _entity,
       std::unordered_set<Entity> &_set);
 
+  /// \brief Recursively erase an entity and all its descendants from a given
+  /// set.
+  /// \param[in] _entity Entity to be erased.
+  /// \param[in, out] _set Set to erase from.
+  public: void EraseEntityRecursive(Entity _entity,
+      std::unordered_set<Entity> &_set);
+
   /// \brief Register a new component type.
   /// \param[in] _typeId Type if of the new component.
   /// \return True if created successfully.
@@ -156,6 +163,9 @@ class ignition::gazebo::EntityComponentManagerPrivate
   /// which belongs the component, and the value is the component being
   /// removed.
   std::unordered_multimap<Entity, ComponentKey> removedComponents;
+
+  /// \brief Set of entities that are prevented from removal.
+  public: std::unordered_set<Entity> lockedEntities;
 };
 
 //////////////////////////////////////////////////
@@ -236,6 +246,17 @@ void EntityComponentManagerPrivate::InsertEntityRecursive(Entity _entity,
   _set.insert(_entity);
 }
 
+/////////////////////////////////////////////////
+void EntityComponentManagerPrivate::EraseEntityRecursive(Entity _entity,
+    std::unordered_set<Entity> &_set)
+{
+  for (const auto &vertex : this->entities.AdjacentsFrom(_entity))
+  {
+    this->EraseEntityRecursive(vertex.first, _set);
+  }
+  _set.erase(_entity);
+}
+
 /////////////////////////////////////////////////
 void EntityComponentManager::RequestRemoveEntity(Entity _entity,
     bool _recursive)
@@ -252,6 +273,22 @@ void EntityComponentManager::RequestRemoveEntity(Entity _entity,
     this->dataPtr->InsertEntityRecursive(_entity, tmpToRemoveEntities);
   }
 
+  // Remove entities from tmpToRemoveEntities that are locked.
+  for (auto iter = tmpToRemoveEntities.begin();
+       iter != tmpToRemoveEntities.end();)
+  {
+    if (std::find(this->dataPtr->lockedEntities.begin(),
+                  this->dataPtr->lockedEntities.end(), *iter) !=
+               this->dataPtr->lockedEntities.end())
+    {
+      iter = tmpToRemoveEntities.erase(iter);
+    }
+    else
+    {
+      ++iter;
+    }
+  }
+
   {
     std::lock_guard<std::mutex> lock(this->dataPtr->entityRemoveMutex);
     this->dataPtr->toRemoveEntities.insert(tmpToRemoveEntities.begin(),
@@ -267,11 +304,41 @@ void EntityComponentManager::RequestRemoveEntity(Entity _entity,
 /////////////////////////////////////////////////
 void EntityComponentManager::RequestRemoveEntities()
 {
+  if (this->dataPtr->lockedEntities.empty())
   {
-    std::lock_guard<std::mutex> lock(this->dataPtr->entityRemoveMutex);
-    this->dataPtr->removeAllEntities = true;
+    {
+      std::lock_guard<std::mutex> lock(this->dataPtr->entityRemoveMutex);
+      this->dataPtr->removeAllEntities = true;
+    }
+    this->RebuildViews();
+  }
+  else
+  {
+    std::unordered_set<Entity> tmpToRemoveEntities;
+
+    // Store the to-be-removed entities in a temporary set so we can call
+    // UpdateViews on each of them
+    for (const auto &vertex : this->dataPtr->entities.Vertices())
+    {
+      if (std::find(this->dataPtr->lockedEntities.begin(),
+                    this->dataPtr->lockedEntities.end(), vertex.first) ==
+          this->dataPtr->lockedEntities.end())
+      {
+        tmpToRemoveEntities.insert(vertex.first);
+      }
+    }
+
+    {
+      std::lock_guard<std::mutex> lock(this->dataPtr->entityRemoveMutex);
+      this->dataPtr->toRemoveEntities.insert(tmpToRemoveEntities.begin(),
+          tmpToRemoveEntities.end());
+    }
+
+    for (const auto &removedEntity : tmpToRemoveEntities)
+    {
+      this->UpdateViews(removedEntity);
+    }
   }
-  this->RebuildViews();
 }
 
 /////////////////////////////////////////////////
@@ -1505,3 +1572,37 @@ void EntityComponentManagerPrivate::AddModifiedComponent(const Entity &_entity)
 
   this->modifiedComponents.insert(_entity);
 }
+
+/////////////////////////////////////////////////
+void EntityComponentManager::LockEntity(const Entity _entity, bool _recursive)
+{
+  if (_recursive)
+  {
+    this->dataPtr->InsertEntityRecursive(_entity,
+        this->dataPtr->lockedEntities);
+  }
+  else
+  {
+    this->dataPtr->lockedEntities.insert(_entity);
+  }
+}
+
+/////////////////////////////////////////////////
+void EntityComponentManager::UnlockEntity(const Entity _entity, bool _recursive)
+{
+  if (_recursive)
+  {
+    this->dataPtr->EraseEntityRecursive(_entity,
+        this->dataPtr->lockedEntities);
+  }
+  else
+  {
+    this->dataPtr->lockedEntities.erase(_entity);
+  }
+}
+
+/////////////////////////////////////////////////
+void EntityComponentManager::UnlockAllEntities()
+{
+  this->dataPtr->lockedEntities.clear();
+}

src/EntityComponentManager_TEST.cc

@@ -2563,6 +2563,66 @@ TEST_P(EntityComponentManagerFixture, RemovedComponentsSyncBetweenServerAndGUI)
   }
 }
 
+//////////////////////////////////////////////////
+TEST_P(EntityComponentManagerFixture, LockEntity)
+{
+  // Create some entities
+  auto e1 = manager.CreateEntity();
+  EXPECT_EQ(1u, e1);
+  EXPECT_TRUE(manager.HasEntity(e1));
+
+  auto e2 = manager.CreateEntity();
+  EXPECT_TRUE(manager.SetParentEntity(e2, e1));
+  EXPECT_EQ(2u, e2);
+  EXPECT_TRUE(manager.HasEntity(e2));
+
+  auto e3 = manager.CreateEntity();
+  EXPECT_EQ(3u, e3);
+  EXPECT_TRUE(manager.HasEntity(e3));
+
+  EXPECT_EQ(3u, manager.EntityCount());
+
+  // Lock e1, which should also lock its child entity e2
+  manager.LockEntity(e1);
+
+  // Try to remove e1, which is locked entity
+  manager.RequestRemoveEntity(e1);
+  EXPECT_EQ(3u, manager.EntityCount());
+  EXPECT_FALSE(manager.HasEntitiesMarkedForRemoval());
+  manager.ProcessEntityRemovals();
+  EXPECT_EQ(3u, manager.EntityCount());
+
+  // Try to remove e2, which has been locked recursively
+  manager.RequestRemoveEntity(e2);
+  EXPECT_EQ(3u, manager.EntityCount());
+  EXPECT_FALSE(manager.HasEntitiesMarkedForRemoval());
+  manager.ProcessEntityRemovals();
+  EXPECT_EQ(3u, manager.EntityCount());
+
+  std::cout << "RequestRemoveEntities\n";
+  // Try to remove all entities, which should leave just e1 and e2
+  manager.RequestRemoveEntities();
+  std::cout << "RequestRemoveEntities Done\n";
+  EXPECT_TRUE(manager.HasEntitiesMarkedForRemoval());
+  manager.ProcessEntityRemovals();
+  EXPECT_EQ(2u, manager.EntityCount());
+
+  // Unlock e2, and now it should be removable.
+  manager.UnlockEntity(e2);
+  manager.RequestRemoveEntity(e2);
+  EXPECT_EQ(2u, manager.EntityCount());
+  EXPECT_TRUE(manager.HasEntitiesMarkedForRemoval());
+  manager.ProcessEntityRemovals();
+  EXPECT_EQ(1u, manager.EntityCount());
+
+  // Unlock all entities, and now it should be removable.
+  manager.UnlockAllEntities();
+  manager.RequestRemoveEntities();
+  EXPECT_TRUE(manager.HasEntitiesMarkedForRemoval());
+  manager.ProcessEntityRemovals();
+  EXPECT_EQ(0u, manager.EntityCount());
+}
+
 // Run multiple times. We want to make sure that static globals don't cause
 // problems.
 INSTANTIATE_TEST_SUITE_P(EntityComponentManagerRepeat,

src/gui/plugins/scene3d/Scene3D.cc

@@ -219,10 +219,10 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE {
     /// only used in lockstep mode and recording in sim time.
     public: std::chrono::steady_clock::time_point recordVideoUpdateTime;
 
-    /// \brief Start tiem of video recording
+    /// \brief Start time of video recording
     public: std::chrono::steady_clock::time_point recordStartTime;
 
-    /// \brief Camera pose publisher
+    /// \brief Video recording statistics publisher
     public: transport::Node::Publisher recorderStatsPub;
 
     /// \brief Target to move the user camera to

src/systems/camera_video_recorder/CameraVideoRecorder.cc

@@ -108,6 +108,22 @@ class ignition::gazebo::systems::CameraVideoRecorderPrivate
 
   /// \brief Topic that the sensor publishes to
   public: std::string sensorTopic;
+
+  /// \brief Video recording statistics publisher
+  public: transport::Node::Publisher recorderStatsPub;
+
+  /// \brief Start time of video recording.
+  public: std::chrono::steady_clock::time_point recordStartTime;
+
+  /// \brief Current simulation time.
+  public: std::chrono::steady_clock::duration simTime{0};
+
+  /// \brief Use sim time as timestamp during video recording
+  /// By default (false), video encoding is done using real time.
+  public: bool recordVideoUseSimTime = false;
+
+  /// \brief Video recorder bitrate (bps)
+  public: unsigned int recordVideoBitrate = 2070000;
 };
 
 //////////////////////////////////////////////////
@@ -224,6 +240,21 @@ void CameraVideoRecorder::Configure(
     }
   }
   this->dataPtr->sensorTopic = topic;
+
+  // Get whether sim time should be used for recording.
+  this->dataPtr->recordVideoUseSimTime = _sdf->Get<bool>("use_sim_time",
+      this->dataPtr->recordVideoUseSimTime).first;
+
+  // Get video recoder bitrate param
+  this->dataPtr->recordVideoBitrate = _sdf->Get<unsigned int>("bitrate",
+      this->dataPtr->recordVideoBitrate).first;
+
+  // recorder stats topic
+  std::string recorderStatsTopic = this->dataPtr->sensorTopic + "/stats";
+  this->dataPtr->recorderStatsPub =
+    this->dataPtr->node.Advertise<msgs::Time>(recorderStatsTopic);
+  ignmsg << "Camera Video recorder stats topic advertised on ["
+    << recorderStatsTopic << "]" << std::endl;
 }
 
 //////////////////////////////////////////////////
@@ -312,8 +343,36 @@ void CameraVideoRecorderPrivate::OnPostRender()
     if (this->videoEncoder.IsEncoding())
     {
       this->camera->Copy(this->cameraImage);
-      this->videoEncoder.AddFrame(
-          this->cameraImage.Data<unsigned char>(), width, height);
+      std::chrono::steady_clock::time_point t;
+        std::chrono::steady_clock::now();
+      if (this->recordVideoUseSimTime)
+        t = std::chrono::steady_clock::time_point(this->simTime);
+      else
+        t = std::chrono::steady_clock::now();
+
+      bool frameAdded = this->videoEncoder.AddFrame(
+          this->cameraImage.Data<unsigned char>(), width, height, t);
+
+      if (frameAdded)
+      {
+        // publish recorder stats
+        if (this->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->recordStartTime = t;
+        }
+
+        std::chrono::steady_clock::duration dt;
+        dt = t - this->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->recorderStatsPub.Publish(msg);
+      }
     }
     // Video recorder is idle. Start recording.
     else
@@ -327,7 +386,11 @@ void CameraVideoRecorderPrivate::OnPostRender()
           &CameraVideoRecorderPrivate::OnImage, this);
 
       this->videoEncoder.Start(this->recordVideoFormat,
-          this->tmpVideoFilename, width, height);
+          this->tmpVideoFilename, width, height, 25,
+          this->recordVideoBitrate);
+
+      this->recordStartTime = std::chrono::steady_clock::time_point(
+            std::chrono::duration(std::chrono::seconds(0)));
 
       ignmsg << "Start video recording on [" << this->service << "]. "
              << "Encoding to tmp file: ["
@@ -362,9 +425,10 @@ void CameraVideoRecorderPrivate::OnPostRender()
 }
 
 //////////////////////////////////////////////////
-void CameraVideoRecorder::PostUpdate(const UpdateInfo &,
+void CameraVideoRecorder::PostUpdate(const UpdateInfo &_info,
     const EntityComponentManager &_ecm)
 {
+  this->dataPtr->simTime = _info.simTime;
   if (!this->dataPtr->cameraName.empty())
     return;
 

src/systems/camera_video_recorder/CameraVideoRecorder.hh

@@ -37,10 +37,14 @@ namespace systems
   **/
   /// \brief Record video from a camera sensor
   /// The system takes in the following parameter:
-  ///   <topic>    Name of topic for the video recorder service. If this is
+  ///   <service>  Name of topic for the video recorder service. If this is
   ///              not specified, the topic defaults to:
   ///              /world/<world_name/model/<model_name>/link/<link_name>/
   ///                  sensor/<sensor_name>/record_video
+  ///   <use_sim_time> True/false value that specifies if the video should
+  //                   be recorded using simulation time or real time. The
+  //                   default is false, which indicates the use of real
+  //                   time.
   class CameraVideoRecorder:
     public System,
     public ISystemConfigure,