Merge branch 'ign-gazebo4' into imu_orientation

Author: Nate Koenig

CMakeLists.txt

@@ -3,7 +3,7 @@ cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR)
 #============================================================================
 # Initialize the project
 #============================================================================
-project(ignition-gazebo4 VERSION 4.10.0)
+project(ignition-gazebo4 VERSION 4.11.0)
 
 #============================================================================
 # Find ignition-cmake

Changelog.md

@@ -1,8 +1,11 @@
 ## Ignition Gazebo 4.x
 
-### Ignition Gazebo 4.x.x (202x-xx-xx)
+### Ignition Gazebo 4.11.x (2021-09-23)
 
-### Ignition Gazebo 4.x.x (2021-09-15)
+1. Support locked entities, and headless video recording using sim time.
+    * [Pull Request 862](https://github.com/ignitionrobotics/ign-gazebo/pull/862)
+
+### Ignition Gazebo 4.10.x (2021-09-15)
 
 1. Fixed GUI's ComponentInspector light parameter
     * [Pull Request 1018](https://github.com/ignitionrobotics/ign-gazebo/pull/1018)
@@ -40,6 +43,14 @@
 1. Using math::SpeedLimiter on the ackermann_steering controller.
     * [Pull Request 837](https://github.com/ignitionrobotics/ign-gazebo/pull/837)
 
+1. All changes merged forward from ign-gazebo3
+    * [Pull Request 866](https://github.com/ignitionrobotics/ign-gazebo/pull/866)
+    * [Pull Request 916](https://github.com/ignitionrobotics/ign-gazebo/pull/916)
+    * [Pull Request 933](https://github.com/ignitionrobotics/ign-gazebo/pull/933)
+    * [Pull Request 946](https://github.com/ignitionrobotics/ign-gazebo/pull/946)
+    * [Pull Request 973](https://github.com/ignitionrobotics/ign-gazebo/pull/973)
+    * [Pull Request 1017](https://github.com/ignitionrobotics/ign-gazebo/pull/1017)
+
 ### Ignition Gazebo 4.9.1 (2021-05-24)
 
 1. Make halt motion act like a brake.

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 pinned.
+      /// \param[in] _recursive Whether to recursively pin all child
+      /// entities. True by default.
+      public: void PinEntity(const Entity _entity, bool _recursive = true);
+
+      /// \brief Allow an entity, and optionally its children, previously
+      /// marked as pinned to be removed.
+      /// \param[in] _entity Entity to be unpinned.
+      /// \param[in] _recursive Whether to recursively unpin all child
+      /// entities. True by default.
+      /// \sa void PinEntity(const Entity, bool)
+      public: void UnpinEntity(const Entity _entity, bool _recursive = true);
+
+      /// \brief Allow all previously pinned entities to be removed.
+      /// \sa void PinEntity(const Entity, bool)
+      public: void UnpinAllEntities();
+
       /// \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> pinnedEntities;
 };
 
 //////////////////////////////////////////////////
@@ -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,23 @@ void EntityComponentManager::RequestRemoveEntity(Entity _entity,
     this->dataPtr->InsertEntityRecursive(_entity, tmpToRemoveEntities);
   }
 
+  // Remove entities from tmpToRemoveEntities that are marked as
+  // unremovable.
+  for (auto iter = tmpToRemoveEntities.begin();
+       iter != tmpToRemoveEntities.end();)
+  {
+    if (std::find(this->dataPtr->pinnedEntities.begin(),
+                  this->dataPtr->pinnedEntities.end(), *iter) !=
+               this->dataPtr->pinnedEntities.end())
+    {
+      iter = tmpToRemoveEntities.erase(iter);
+    }
+    else
+    {
+      ++iter;
+    }
+  }
+
   {
     std::lock_guard<std::mutex> lock(this->dataPtr->entityRemoveMutex);
     this->dataPtr->toRemoveEntities.insert(tmpToRemoveEntities.begin(),
@@ -267,11 +305,41 @@ void EntityComponentManager::RequestRemoveEntity(Entity _entity,
 /////////////////////////////////////////////////
 void EntityComponentManager::RequestRemoveEntities()
 {
+  if (this->dataPtr->pinnedEntities.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->pinnedEntities.begin(),
+                    this->dataPtr->pinnedEntities.end(), vertex.first) ==
+          this->dataPtr->pinnedEntities.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();
 }
 
 /////////////////////////////////////////////////
@@ -1514,3 +1582,37 @@ void EntityComponentManagerPrivate::AddModifiedComponent(const Entity &_entity)
 
   this->modifiedComponents.insert(_entity);
 }
+
+/////////////////////////////////////////////////
+void EntityComponentManager::PinEntity(const Entity _entity, bool _recursive)
+{
+  if (_recursive)
+  {
+    this->dataPtr->InsertEntityRecursive(_entity,
+        this->dataPtr->pinnedEntities);
+  }
+  else
+  {
+    this->dataPtr->pinnedEntities.insert(_entity);
+  }
+}
+
+/////////////////////////////////////////////////
+void EntityComponentManager::UnpinEntity(const Entity _entity, bool _recursive)
+{
+  if (_recursive)
+  {
+    this->dataPtr->EraseEntityRecursive(_entity,
+        this->dataPtr->pinnedEntities);
+  }
+  else
+  {
+    this->dataPtr->pinnedEntities.erase(_entity);
+  }
+}
+
+/////////////////////////////////////////////////
+void EntityComponentManager::UnpinAllEntities()
+{
+  this->dataPtr->pinnedEntities.clear();
+}

src/EntityComponentManager_TEST.cc

@@ -2571,6 +2571,64 @@ TEST_P(EntityComponentManagerFixture, RemovedComponentsSyncBetweenServerAndGUI)
   }
 }
 
+//////////////////////////////////////////////////
+TEST_P(EntityComponentManagerFixture, PinnedEntity)
+{
+  // 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());
+
+  // Mark e1 as unremovable, which should also lock its child entity e2
+  manager.PinEntity(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());
+
+  // Try to remove all entities, which should leave just e1 and e2
+  manager.RequestRemoveEntities();
+  EXPECT_TRUE(manager.HasEntitiesMarkedForRemoval());
+  manager.ProcessEntityRemovals();
+  EXPECT_EQ(2u, manager.EntityCount());
+
+  // Unmark e2, and now it should be removable.
+  manager.UnpinEntity(e2);
+  manager.RequestRemoveEntity(e2);
+  EXPECT_EQ(2u, manager.EntityCount());
+  EXPECT_TRUE(manager.HasEntitiesMarkedForRemoval());
+  manager.ProcessEntityRemovals();
+  EXPECT_EQ(1u, manager.EntityCount());
+
+  // Unmark all entities, and now it should be removable.
+  manager.UnpinAllEntities();
+  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

@@ -157,10 +157,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