Add test for EntityFeatureMap

src/systems/physics/CMakeLists.txt

@@ -7,13 +7,13 @@ gz_add_system(physics
     ignition-physics${IGN_PHYSICS_VER}::ignition-physics${IGN_PHYSICS_VER}
 )
 
-# set (gtest_sources
-#   EntityFeatureMap_TEST.cc
-# )
+set (gtest_sources
+  EntityFeatureMap_TEST.cc
+)
 
-# ign_build_tests(TYPE UNIT
-#   SOURCES
-#   ${gtest_sources}
-#   LIB_DEPS
-#   ignition-physics${IGN_PHYSICS_VER}::core
-# )
+ign_build_tests(TYPE UNIT
+  SOURCES
+  ${gtest_sources}
+  LIB_DEPS
+  ignition-physics${IGN_PHYSICS_VER}::core
+)

src/systems/physics/EntityFeatureMap.hh

@@ -43,6 +43,16 @@ namespace systems::physics_system
   // This ensures that reference counts are properly zeroed out in the
   // underlying physics engines and the memory associated with the physics
   // entities can be freed.
+  //
+  // DEV WARNING: There is an implicit conversion between physics EntityPtr and
+  // std::size_t in ign-physics. This seems also implicitly convert between
+  // EntityPtr and gazebo Entity. Therefore, any member function that takes a
+  // gazebo Entity can accidentally take an EntityPtr. To prevent this, for
+  // every function that takes a gazebo Entity, we should always have an
+  // overload that also takes an EntityPtr with required features. We can do
+  // this because there's a 1:1 mapping between the two in maps contained in
+  // this class.
+  //
   // \tparam PhysicsEntityT Type of entity, such as World, Model, or Link
   // \tparam PolicyT Policy of the physics engine (2D, 3D)
   // \tparam RequiredFeatureList Required features of the physics entity
@@ -84,7 +94,8 @@ namespace systems::physics_system
     /// entity can't be found or the physics engine doesn't support the
     /// requested feature.
     public: template <typename ToFeatureList>
-            PhysicsEntityPtr<ToFeatureList> EntityCast(Entity _entity)
+            PhysicsEntityPtr<ToFeatureList>
+            EntityCast(gazebo::Entity _entity) const
     {
       // Using constexpr to limit compiler error message to the static_assert
       // cppcheck-suppress syntaxError
@@ -127,6 +138,25 @@ namespace systems::physics_system
         return castEntity;
       }
     }
+    /// \brief Helper function to cast from an entity type with minimum features
+    /// to an entity with a different set of features. This overload takes a
+    /// physics entity as input
+    /// \tparam ToFeatureList The list of features of the resulting entity.
+    /// \param[in] _entity Physics entity with required features.
+    /// \return Physics entity with features in ToFeatureList. nullptr if the
+    /// entity can't be found or the physics engine doesn't support the
+    /// requested feature.
+    public: template <typename ToFeatureList>
+            PhysicsEntityPtr<ToFeatureList>
+            EntityCast(const RequiredEntityPtr &_physicsEntity) const
+    {
+      auto gzEntity = this->Get(_physicsEntity);
+      if (kNullEntity == gzEntity)
+      {
+        return nullptr;
+      }
+      return this->EntityCast<ToFeatureList>(gzEntity);
+    }
 
     /// \brief Get the physics entity with required features that corresponds to
     /// the input Gazebo entity
@@ -168,11 +198,21 @@ namespace systems::physics_system
       return this->entityMap.find(_entity) != this->entityMap.end();
     }
 
+    /// \brief Check whether there is a gazebo entity associated with the given
+    /// physics entity
+    /// \param[in] _physicsEntity physics entity with required features.
+    /// \return True if the there is a gazebo entity associated with the given
+    /// physics entity
+    public: bool HasEntity(const RequiredEntityPtr &_physicsEntity) const
+    {
+      return this->reverseMap.find(_physicsEntity) != this->reverseMap.end();
+    }
+
     /// \brief Add a mapping between gazebo and physics entities
     /// \param[in] _entity Gazebo entity.
     /// \param[in] _physicsEntity Physics entity with required feature
     public: void AddEntity(const Entity &_entity,
-                           RequiredEntityPtr _physicsEntity)
+                           const RequiredEntityPtr &_physicsEntity)
     {
       this->entityMap[_entity] = _physicsEntity;
       this->reverseMap[_physicsEntity] = _entity;
@@ -194,6 +234,22 @@ namespace systems::physics_system
       return false;
     }
 
+    /// \brief Remove physics entity from all associated maps
+    /// \param[in] _entity Gazebo entity.
+    /// \return True if the entity was found and removed.
+    public: bool Remove(const RequiredEntityPtr &_physicsEntity)
+    {
+      auto it = this->reverseMap.find(_physicsEntity);
+      if (it != this->reverseMap.end())
+      {
+        this->entityMap.erase(it->second);
+        this->reverseMap.erase(it);
+        this->castCache.erase(it->second);
+        return true;
+      }
+      return false;
+    }
+
     /// \brief Get the map from Gazebo entity to physics entities with required
     /// features
     /// \return Immumtable entity map
@@ -202,6 +258,15 @@ namespace systems::physics_system
       return this->entityMap;
     }
 
+    /// \brief Get the total number of entries in the three maps. Only used for
+    /// testing.
+    /// \return Number of entries in all the maps.
+    public: std::size_t TotalMapEntryCount() const
+    {
+      return this->entityMap.size() + this->reverseMap.size() +
+             this->castCache.size();
+    }
+
     /// \brief Map from Gazebo entity to physics entities with required features
     private: std::unordered_map<Entity, RequiredEntityPtr> entityMap;
 
@@ -210,7 +275,7 @@ namespace systems::physics_system
 
     /// \brief Cache map from Gazebo entity to physics entities with optional
     /// features
-    private: std::unordered_map<Entity, ValueType> castCache;
+    private: mutable std::unordered_map<Entity, ValueType> castCache;
   };
 
   /// \brief Convenience template that presets EntityFeatureMap with

src/systems/physics/EntityFeatureMap_TEST.cc

@@ -0,0 +1,173 @@
+/*
+ * Copyright (C) 2021 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+ */
+
+#include "EntityFeatureMap.hh"
+
+#include <gtest/gtest.h>
+
+#include <ignition/physics/BoxShape.hh>
+#include <ignition/physics/CylinderShape.hh>
+#include <ignition/physics/ConstructEmpty.hh>
+#include <ignition/physics/Entity.hh>
+#include <ignition/physics/ForwardStep.hh>
+#include <ignition/physics/Implements.hh>
+#include <ignition/physics/Link.hh>
+#include <ignition/physics/RemoveEntities.hh>
+#include <ignition/physics/config.hh>
+#include <ignition/plugin/Loader.hh>
+
+#include "ignition/gazebo/EntityComponentManager.hh"
+
+using namespace ignition;
+using namespace ignition::gazebo::systems::physics_system;
+
+struct MinimumFeatureList
+    : physics::FeatureList<physics::ConstructEmptyWorldFeature,
+                           physics::ConstructEmptyModelFeature,
+                           physics::ConstructEmptyLinkFeature>
+{
+};
+
+using EnginePtrType =
+    physics::EnginePtr<physics::FeaturePolicy3d, MinimumFeatureList>;
+
+
+class EntityFeatureMapFixture: public ::testing::Test
+{
+  protected: void SetUp() override
+  {
+    const std::string pluginLib = "libignition-physics-dartsim-plugin.so";
+
+    common::SystemPaths systemPaths;
+    systemPaths.AddPluginPaths({IGNITION_PHYSICS_ENGINE_INSTALL_DIR});
+
+    auto pathToLib = systemPaths.FindSharedLibrary(pluginLib);
+    ASSERT_FALSE(pathToLib.empty())
+        << "Failed to find plugin [" << pluginLib << "]";
+
+    // Load engine plugin
+    ignition::plugin::Loader pluginLoader;
+    auto plugins = pluginLoader.LoadLib(pathToLib);
+    ASSERT_FALSE(plugins.empty())
+        << "Unable to load the [" << pathToLib << "] library.";
+
+    auto classNames = pluginLoader.AllPlugins();
+    ASSERT_FALSE(classNames.empty())
+        << "No plugins found in library [" << pathToLib << "].";
+
+    for (const auto &className : classNames)
+    {
+      // Get the first plugin that works
+      auto plugin = pluginLoader.Instantiate(className);
+
+      ASSERT_TRUE(plugin) << "Class " << className << " not found in library ["
+                          << pathToLib << "]";
+
+      this->engine =
+          ignition::physics::RequestEngine<physics::FeaturePolicy3d,
+                                           MinimumFeatureList>::From(plugin);
+
+      ASSERT_NE(nullptr, this->engine);
+      break;
+    }
+  }
+
+  public: EnginePtrType engine;
+};
+
+TEST_F(EntityFeatureMapFixture, AddCastRemoveEntity)
+{
+  struct TestOptionalFeatures1
+      : physics::FeatureList<physics::LinkFrameSemantics>
+  {
+  };
+  using TestOptionalFeatures2 = physics::FeatureList<physics::RemoveEntities>;
+
+  using WorldEntityMap =
+      EntityFeatureMap3d<physics::World, MinimumFeatureList,
+                         TestOptionalFeatures1, TestOptionalFeatures2>;
+
+  using WorldPtrType = physics::EntityPtr<
+      physics::World<physics::FeaturePolicy3d, MinimumFeatureList>>;
+
+  // Making these entities different from 1 and 2 ensures that the implicit
+  // conversion in ign-physics between EntityPtr and std::size_t doesn't cause
+  // false positive tests
+  gazebo::Entity gazeboWorld1Entity = 123;
+  gazebo::Entity gazeboWorld2Entity = 456;
+  WorldPtrType testWorld1 = this->engine->ConstructEmptyWorld("world1");
+  WorldEntityMap testMap;
+  EXPECT_FALSE(testMap.HasEntity(gazeboWorld1Entity));
+  EXPECT_EQ(nullptr, testMap.Get(gazeboWorld1Entity));
+  EXPECT_EQ(gazebo::kNullEntity, testMap.Get(testWorld1));
+  EXPECT_EQ(0u, testMap.TotalMapEntryCount());
+
+  testMap.AddEntity(gazeboWorld1Entity, testWorld1);
+
+  // After adding the entity, there should be one entry each in two maps
+  EXPECT_EQ(2u, testMap.TotalMapEntryCount());
+  EXPECT_EQ(testWorld1, testMap.Get(gazeboWorld1Entity));
+  EXPECT_EQ(gazeboWorld1Entity, testMap.Get(testWorld1));
+
+  // Cast to optional feature1
+  auto testWorld1Feature1 =
+      testMap.EntityCast<TestOptionalFeatures1>(gazeboWorld1Entity);
+  ASSERT_NE(nullptr, testWorld1Feature1);
+  // After the cast, there should be one more entry in the cache map.
+  EXPECT_EQ(3u, testMap.TotalMapEntryCount());
+
+  // Cast to optional feature2
+  auto testWorld1Feature2 =
+      testMap.EntityCast<TestOptionalFeatures2>(gazeboWorld1Entity);
+  ASSERT_NE(nullptr, testWorld1Feature2);
+  // After the cast, the number of entries should remain the same because we
+  // have not added an entity.
+  EXPECT_EQ(3u, testMap.TotalMapEntryCount());
+
+  // Add another entity
+  WorldPtrType testWorld2 = this->engine->ConstructEmptyWorld("world2");
+  testMap.AddEntity(gazeboWorld2Entity, testWorld2);
+  EXPECT_EQ(5u, testMap.TotalMapEntryCount());
+  EXPECT_EQ(testWorld2, testMap.Get(gazeboWorld2Entity));
+  EXPECT_EQ(gazeboWorld2Entity, testMap.Get(testWorld2));
+
+  auto testWorld2Feature1 =
+      testMap.EntityCast<TestOptionalFeatures1>(testWorld2);
+  ASSERT_NE(nullptr, testWorld2Feature1);
+  // After the cast, there should be one more entry in the cache map.
+  EXPECT_EQ(6u, testMap.TotalMapEntryCount());
+
+  auto testWorld2Feature2 =
+      testMap.EntityCast<TestOptionalFeatures2>(testWorld2);
+  ASSERT_NE(nullptr, testWorld2Feature2);
+  // After the cast, the number of entries should remain the same because we
+  // have not added an entity.
+  EXPECT_EQ(6u, testMap.TotalMapEntryCount());
+
+  // Remove entitites
+  testMap.Remove(gazeboWorld1Entity);
+  EXPECT_FALSE(testMap.HasEntity(gazeboWorld1Entity));
+  EXPECT_EQ(nullptr, testMap.Get(gazeboWorld1Entity));
+  EXPECT_EQ(gazebo::kNullEntity, testMap.Get(testWorld1));
+  EXPECT_EQ(3u, testMap.TotalMapEntryCount());
+
+  testMap.Remove(testWorld2);
+  EXPECT_FALSE(testMap.HasEntity(gazeboWorld2Entity));
+  EXPECT_EQ(nullptr, testMap.Get(gazeboWorld2Entity));
+  EXPECT_EQ(gazebo::kNullEntity, testMap.Get(testWorld2));
+  EXPECT_EQ(0u, testMap.TotalMapEntryCount());
+}